Merge branch 'for-3.7' of git://git.infradead.org/users/dedekind/l2-mtd

Conflicts:
	drivers/mtd/nand/nand_base.c

944 files changed, 34674 insertions, 17239 deletions

diff --git a/drivers/Makefile b/drivers/Makefile
index 03da5b663aef..a16a8d001ae0 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -17,6 +17,10 @@ obj-$(CONFIG_PARISC)		+= parisc/
 obj-$(CONFIG_RAPIDIO)           += rapidio/
 obj-y                           += video/
 obj-y                           += idle/
+
+# IPMI must come before ACPI in order to provide IPMI opregion support
+obj-$(CONFIG_IPMI_HANDLER)      += char/ipmi/
+
 obj-$(CONFIG_ACPI)              += acpi/
 obj-$(CONFIG_SFI)               += sfi/
 # PnP must come after ACPI since it will eventually need to check if acpi
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 47199e2a9130..82422fe90f81 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -47,6 +47,10 @@ acpi-y				+= video_detect.o
 endif
 
 # These are (potentially) separate modules
+
+# IPMI may be used by other drivers, so it has to initialise before them
+obj-$(CONFIG_ACPI_IPMI)         += acpi_ipmi.o
+
 obj-$(CONFIG_ACPI_AC)           += ac.o
 obj-$(CONFIG_ACPI_BUTTON)       += button.o
 obj-$(CONFIG_ACPI_FAN)          += fan.o
@@ -70,6 +74,5 @@ processor-y			+= processor_idle.o processor_thermal.o
 processor-$(CONFIG_CPU_FREQ)    += processor_perflib.o
 
 obj-$(CONFIG_ACPI_PROCESSOR_AGGREGATOR) += acpi_pad.o
-obj-$(CONFIG_ACPI_IPMI)         += acpi_ipmi.o
 
 obj-$(CONFIG_ACPI_APEI)         += apei/
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index 7edaccce6640..a51df9681319 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -71,9 +71,6 @@ enum ec_command {
 #define ACPI_EC_UDELAY_GLK      1000    /* Wait 1ms max. to get global lock */
 #define ACPI_EC_MSI_UDELAY      550     /* Wait 550us for MSI EC */
 
-#define ACPI_EC_STORM_THRESHOLD 8       /* number of false interrupts
-                                           per one transaction */
-
 enum {
         EC_FLAGS_QUERY_PENDING,         /* Query is pending */
         EC_FLAGS_GPE_STORM,             /* GPE storm detected */
@@ -87,6 +84,15 @@ static unsigned int ec_delay __read_mostly = ACPI_EC_DELAY;
 module_param(ec_delay, uint, 0644);
 MODULE_PARM_DESC(ec_delay, "Timeout(ms) waited until an EC command completes");
 
+/*
+ * If the number of false interrupts per one transaction exceeds
+ * this threshold, will think there is a GPE storm happened and
+ * will disable the GPE for normal transaction.
+ */
+static unsigned int ec_storm_threshold  __read_mostly = 8;
+module_param(ec_storm_threshold, uint, 0644);
+MODULE_PARM_DESC(ec_storm_threshold, "Maxim false GPE numbers not considered as GPE storm");
+
 /* If we find an EC via the ECDT, we need to keep a ptr to its context */
 /* External interfaces use first EC only, so remember */
 typedef int (*acpi_ec_query_func) (void *data);
@@ -319,7 +325,7 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
                 msleep(1);
                 /* It is safe to enable the GPE outside of the transaction. */
                 acpi_enable_gpe(NULL, ec->gpe);
-        } else if (t->irq_count > ACPI_EC_STORM_THRESHOLD) {
+        } else if (t->irq_count > ec_storm_threshold) {
                 pr_info(PREFIX "GPE storm detected, "
                         "transactions will use polling mode\n");
                 set_bit(EC_FLAGS_GPE_STORM, &ec->flags);
@@ -924,6 +930,17 @@ static int ec_flag_msi(const struct dmi_system_id *id)
         return 0;
 }
 
+/*
+ * Clevo M720 notebook actually works ok with IRQ mode, if we lifted
+ * the GPE storm threshold back to 20
+ */
+static int ec_enlarge_storm_threshold(const struct dmi_system_id *id)
+{
+        pr_debug("Setting the EC GPE storm threshold to 20\n");
+        ec_storm_threshold  = 20;
+        return 0;
+}
+
 static struct dmi_system_id __initdata ec_dmi_table[] = {
         {
         ec_skip_dsdt_scan, "Compal JFL92", {
@@ -955,10 +972,13 @@ static struct dmi_system_id __initdata ec_dmi_table[] = {
         {
         ec_validate_ecdt, "ASUS hardware", {
         DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer Inc.") }, NULL},
+        {
+        ec_enlarge_storm_threshold, "CLEVO hardware", {
+        DMI_MATCH(DMI_SYS_VENDOR, "CLEVO Co."),
+        DMI_MATCH(DMI_PRODUCT_NAME, "M720T/M730T"),}, NULL},
         {},
 };
 
-
 int __init acpi_ec_ecdt_probe(void)
 {
         acpi_status status;
diff --git a/drivers/acpi/glue.c b/drivers/acpi/glue.c
index d1a2d74033e9..08373086cd7e 100644
--- a/drivers/acpi/glue.c
+++ b/drivers/acpi/glue.c
@@ -159,6 +159,7 @@ static int acpi_bind_one(struct device *dev, acpi_handle handle)
         if (physical_node->node_id >= ACPI_MAX_PHYSICAL_NODE) {
                 retval = -ENOSPC;
                 mutex_unlock(&acpi_dev->physical_node_lock);
+                kfree(physical_node);
                 goto err;
         }
 
diff --git a/drivers/acpi/processor_driver.c b/drivers/acpi/processor_driver.c
index e78c2a52ea46..bd4e5dca3ff7 100644
--- a/drivers/acpi/processor_driver.c
+++ b/drivers/acpi/processor_driver.c
@@ -409,6 +409,7 @@ static void acpi_processor_notify(struct acpi_device *device, u32 event)
                 acpi_bus_generate_proc_event(device, event, 0);
                 acpi_bus_generate_netlink_event(device->pnp.device_class,
                                                   dev_name(&device->dev), event, 0);
+                break;
         default:
                 ACPI_DEBUG_PRINT((ACPI_DB_INFO,
                                   "Unsupported event [0x%x]\n", event));
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 3655ab923812..e8086c725305 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -1132,7 +1132,7 @@ static int acpi_processor_setup_cpuidle_states(struct acpi_processor *pr)
 int acpi_processor_hotplug(struct acpi_processor *pr)
 {
         int ret = 0;
-        struct cpuidle_device *dev = per_cpu(acpi_cpuidle_device, pr->id);
+        struct cpuidle_device *dev;
 
         if (disabled_by_idle_boot_param())
                 return 0;
@@ -1147,6 +1147,7 @@ int acpi_processor_hotplug(struct acpi_processor *pr)
         if (!pr->flags.power_setup_done)
                 return -ENODEV;
 
+        dev = per_cpu(acpi_cpuidle_device, pr->id);
         cpuidle_pause_and_lock();
         cpuidle_disable_device(dev);
         acpi_processor_get_power_info(pr);
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index edda74a43406..804204d41999 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -708,6 +708,40 @@ static int thermal_get_crit_temp(struct thermal_zone_device *thermal,
                 return -EINVAL;
 }
 
+static int thermal_get_trend(struct thermal_zone_device *thermal,
+                                int trip, enum thermal_trend *trend)
+{
+        struct acpi_thermal *tz = thermal->devdata;
+        enum thermal_trip_type type;
+        int i;
+
+        if (thermal_get_trip_type(thermal, trip, &type))
+                return -EINVAL;
+
+        if (type == THERMAL_TRIP_ACTIVE) {
+                /* aggressive active cooling */
+                *trend = THERMAL_TREND_RAISING;
+                return 0;
+        }
+
+        /*
+         * tz->temperature has already been updated by generic thermal layer,
+         * before this callback being invoked
+         */
+        i = (tz->trips.passive.tc1 * (tz->temperature - tz->last_temperature))
+                + (tz->trips.passive.tc2
+                * (tz->temperature - tz->trips.passive.temperature));
+
+        if (i > 0)
+                *trend = THERMAL_TREND_RAISING;
+        else if (i < 0)
+                *trend = THERMAL_TREND_DROPPING;
+        else
+                *trend = THERMAL_TREND_STABLE;
+        return 0;
+}
+
+
 static int thermal_notify(struct thermal_zone_device *thermal, int trip,
                            enum thermal_trip_type trip_type)
 {
@@ -731,11 +765,9 @@ static int thermal_notify(struct thermal_zone_device *thermal, int trip,
         return 0;
 }
 
-typedef int (*cb)(struct thermal_zone_device *, int,
-                  struct thermal_cooling_device *);
 static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
                                         struct thermal_cooling_device *cdev,
-                                        cb action)
+                                        bool bind)
 {
         struct acpi_device *device = cdev->devdata;
         struct acpi_thermal *tz = thermal->devdata;
@@ -759,11 +791,19 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
                     i++) {
                         handle = tz->trips.passive.devices.handles[i];
                         status = acpi_bus_get_device(handle, &dev);
-                        if (ACPI_SUCCESS(status) && (dev == device)) {
-                                result = action(thermal, trip, cdev);
-                                if (result)
-                                        goto failed;
-                        }
+                        if (ACPI_FAILURE(status) || dev != device)
+                                continue;
+                        if (bind)
+                                result =
+                                        thermal_zone_bind_cooling_device
+                                        (thermal, trip, cdev,
+                                         THERMAL_NO_LIMIT, THERMAL_NO_LIMIT);
+                        else
+                                result =
+                                        thermal_zone_unbind_cooling_device
+                                        (thermal, trip, cdev);
+                        if (result)
+                                goto failed;
                 }
         }
 
@@ -776,11 +816,17 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
                     j++) {
                         handle = tz->trips.active[i].devices.handles[j];
                         status = acpi_bus_get_device(handle, &dev);
-                        if (ACPI_SUCCESS(status) && (dev == device)) {
-                                result = action(thermal, trip, cdev);
-                                if (result)
-                                        goto failed;
-                        }
+                        if (ACPI_FAILURE(status) || dev != device)
+                                continue;
+                        if (bind)
+                                result = thermal_zone_bind_cooling_device
+                                        (thermal, trip, cdev,
+                                         THERMAL_NO_LIMIT, THERMAL_NO_LIMIT);
+                        else
+                                result = thermal_zone_unbind_cooling_device
+                                        (thermal, trip, cdev);
+                        if (result)
+                                goto failed;
                 }
         }
 
@@ -788,7 +834,14 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
                 handle = tz->devices.handles[i];
                 status = acpi_bus_get_device(handle, &dev);
                 if (ACPI_SUCCESS(status) && (dev == device)) {
-                        result = action(thermal, -1, cdev);
+                        if (bind)
+                                result = thermal_zone_bind_cooling_device
+                                                (thermal, -1, cdev,
+                                                 THERMAL_NO_LIMIT,
+                                                 THERMAL_NO_LIMIT);
+                        else
+                                result = thermal_zone_unbind_cooling_device
+                                                (thermal, -1, cdev);
                         if (result)
                                 goto failed;
                 }
@@ -802,16 +855,14 @@ static int
 acpi_thermal_bind_cooling_device(struct thermal_zone_device *thermal,
                                         struct thermal_cooling_device *cdev)
 {
-        return acpi_thermal_cooling_device_cb(thermal, cdev,
-                                thermal_zone_bind_cooling_device);
+        return acpi_thermal_cooling_device_cb(thermal, cdev, true);
 }
 
 static int
 acpi_thermal_unbind_cooling_device(struct thermal_zone_device *thermal,
                                         struct thermal_cooling_device *cdev)
 {
-        return acpi_thermal_cooling_device_cb(thermal, cdev,
-                                thermal_zone_unbind_cooling_device);
+        return acpi_thermal_cooling_device_cb(thermal, cdev, false);
 }
 
 static const struct thermal_zone_device_ops acpi_thermal_zone_ops = {
@@ -823,6 +874,7 @@ static const struct thermal_zone_device_ops acpi_thermal_zone_ops = {
         .get_trip_type = thermal_get_trip_type,
         .get_trip_temp = thermal_get_trip_temp,
         .get_crit_temp = thermal_get_crit_temp,
+        .get_trend = thermal_get_trend,
         .notify = thermal_notify,
 };
 
@@ -849,15 +901,12 @@ static int acpi_thermal_register_thermal_zone(struct acpi_thermal *tz)
                 tz->thermal_zone =
                         thermal_zone_device_register("acpitz", trips, 0, tz,
                                                      &acpi_thermal_zone_ops,
-                                                     tz->trips.passive.tc1,
-                                                     tz->trips.passive.tc2,
                                                      tz->trips.passive.tsp*100,
                                                      tz->polling_frequency*100);
         else
                 tz->thermal_zone =
                         thermal_zone_device_register("acpitz", trips, 0, tz,
-                                                     &acpi_thermal_zone_ops,
-                                                     0, 0, 0,
+                                                     &acpi_thermal_zone_ops, 0,
                                                      tz->polling_frequency*100);
         if (IS_ERR(tz->thermal_zone))
                 return -ENODEV;
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index f94d4c818fc7..0230cb6cbb3a 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -1345,12 +1345,15 @@ static int
 acpi_video_bus_get_devices(struct acpi_video_bus *video,
                            struct acpi_device *device)
 {
-        int status;
+        int status = 0;
         struct acpi_device *dev;
 
-        status = acpi_video_device_enumerate(video);
-        if (status)
-                return status;
+        /*
+         * There are systems where video module known to work fine regardless
+         * of broken _DOD and ignoring returned value here doesn't cause
+         * any issues later.
+         */
+        acpi_video_device_enumerate(video);
 
         list_for_each_entry(dev, &device->children, node) {
 
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 08b4c5209384..b34b5cda5ae1 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -236,7 +236,7 @@ config CMA_SIZE_PERCENTAGE
 
 choice
         prompt "Selected region size"
-        default CMA_SIZE_SEL_ABSOLUTE
+        default CMA_SIZE_SEL_MBYTES
 
 config CMA_SIZE_SEL_MBYTES
         bool "Use mega bytes value only"
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index 560a7173f810..bc256b641027 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -191,9 +191,8 @@ EXPORT_SYMBOL(dma_release_from_coherent);
  * This checks whether the memory was allocated from the per-device
  * coherent memory pool and if so, maps that memory to the provided vma.
  *
- * Returns 1 if we correctly mapped the memory, or 0 if
- * dma_release_coherent() should proceed with mapping memory from
- * generic pools.
+ * Returns 1 if we correctly mapped the memory, or 0 if the caller should
+ * proceed with mapping memory from generic pools.
  */
 int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
                            void *vaddr, size_t size, int *ret)
diff --git a/drivers/base/dma-contiguous.c b/drivers/base/dma-contiguous.c
index 9a1469474f55..612afcc5a938 100644
--- a/drivers/base/dma-contiguous.c
+++ b/drivers/base/dma-contiguous.c
@@ -27,15 +27,12 @@
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <linux/page-isolation.h>
+#include <linux/sizes.h>
 #include <linux/slab.h>
 #include <linux/swap.h>
 #include <linux/mm_types.h>
 #include <linux/dma-contiguous.h>
 
-#ifndef SZ_1M
-#define SZ_1M (1 << 20)
-#endif
-
 struct cma {
         unsigned long   base_pfn;
         unsigned long   count;
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 81541452887b..8945f4e489ed 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -36,68 +36,6 @@ MODULE_AUTHOR("Manuel Estrada Sainz");
 MODULE_DESCRIPTION("Multi purpose firmware loading support");
 MODULE_LICENSE("GPL");
 
-static const char *fw_path[] = {
-        "/lib/firmware/updates/" UTS_RELEASE,
-        "/lib/firmware/updates",
-        "/lib/firmware/" UTS_RELEASE,
-        "/lib/firmware"
-};
-
-/* Don't inline this: 'struct kstat' is biggish */
-static noinline long fw_file_size(struct file *file)
-{
-        struct kstat st;
-        if (vfs_getattr(file->f_path.mnt, file->f_path.dentry, &st))
-                return -1;
-        if (!S_ISREG(st.mode))
-                return -1;
-        if (st.size != (long)st.size)
-                return -1;
-        return st.size;
-}
-
-static bool fw_read_file_contents(struct file *file, struct firmware *fw)
-{
-        long size;
-        char *buf;
-
-        size = fw_file_size(file);
-        if (size < 0)
-                return false;
-        buf = vmalloc(size);
-        if (!buf)
-                return false;
-        if (kernel_read(file, 0, buf, size) != size) {
-                vfree(buf);
-                return false;
-        }
-        fw->data = buf;
-        fw->size = size;
-        return true;
-}
-
-static bool fw_get_filesystem_firmware(struct firmware *fw, const char *name)
-{
-        int i;
-        bool success = false;
-        char *path = __getname();
-
-        for (i = 0; i < ARRAY_SIZE(fw_path); i++) {
-                struct file *file;
-                snprintf(path, PATH_MAX, "%s/%s", fw_path[i], name);
-
-                file = filp_open(path, O_RDONLY, 0);
-                if (IS_ERR(file))
-                        continue;
-                success = fw_read_file_contents(file, fw);
-                fput(file);
-                if (success)
-                        break;
-        }
-        __putname(path);
-        return success;
-}
-
 /* Builtin firmware support */
 
 #ifdef CONFIG_FW_LOADER
@@ -150,6 +88,11 @@ enum {
         FW_STATUS_ABORT,
 };
 
+enum fw_buf_fmt {
+        VMALLOC_BUF,    /* used in direct loading */
+        PAGE_BUF,       /* used in loading via userspace */
+};
+
 static int loading_timeout = 60;        /* In seconds */
 
 static inline long firmware_loading_timeout(void)
@@ -173,8 +116,6 @@ struct firmware_cache {
         spinlock_t name_lock;
         struct list_head fw_names;
 
-        wait_queue_head_t wait_queue;
-        int cnt;
         struct delayed_work work;
 
         struct notifier_block   pm_notify;
@@ -187,6 +128,7 @@ struct firmware_buf {
         struct completion completion;
         struct firmware_cache *fwc;
         unsigned long status;
+        enum fw_buf_fmt fmt;
         void *data;
         size_t size;
         struct page **pages;
@@ -240,6 +182,7 @@ static struct firmware_buf *__allocate_fw_buf(const char *fw_name,
         strcpy(buf->fw_id, fw_name);
         buf->fwc = fwc;
         init_completion(&buf->completion);
+        buf->fmt = VMALLOC_BUF;
 
         pr_debug("%s: fw-%s buf=%p\n", __func__, fw_name, buf);
 
@@ -307,10 +250,14 @@ static void __fw_free_buf(struct kref *ref)
         list_del(&buf->list);
         spin_unlock(&fwc->lock);
 
-        vunmap(buf->data);
-        for (i = 0; i < buf->nr_pages; i++)
-                __free_page(buf->pages[i]);
-        kfree(buf->pages);
+
+        if (buf->fmt == PAGE_BUF) {
+                vunmap(buf->data);
+                for (i = 0; i < buf->nr_pages; i++)
+                        __free_page(buf->pages[i]);
+                kfree(buf->pages);
+        } else
+                vfree(buf->data);
         kfree(buf);
 }
 
@@ -319,6 +266,69 @@ static void fw_free_buf(struct firmware_buf *buf)
         kref_put(&buf->ref, __fw_free_buf);
 }
 
+/* direct firmware loading support */
+static const char *fw_path[] = {
+        "/lib/firmware/updates/" UTS_RELEASE,
+        "/lib/firmware/updates",
+        "/lib/firmware/" UTS_RELEASE,
+        "/lib/firmware"
+};
+
+/* Don't inline this: 'struct kstat' is biggish */
+static noinline long fw_file_size(struct file *file)
+{
+        struct kstat st;
+        if (vfs_getattr(file->f_path.mnt, file->f_path.dentry, &st))
+                return -1;
+        if (!S_ISREG(st.mode))
+                return -1;
+        if (st.size != (long)st.size)
+                return -1;
+        return st.size;
+}
+
+static bool fw_read_file_contents(struct file *file, struct firmware_buf *fw_buf)
+{
+        long size;
+        char *buf;
+
+        size = fw_file_size(file);
+        if (size < 0)
+                return false;
+        buf = vmalloc(size);
+        if (!buf)
+                return false;
+        if (kernel_read(file, 0, buf, size) != size) {
+                vfree(buf);
+                return false;
+        }
+        fw_buf->data = buf;
+        fw_buf->size = size;
+        return true;
+}
+
+static bool fw_get_filesystem_firmware(struct firmware_buf *buf)
+{
+        int i;
+        bool success = false;
+        char *path = __getname();
+
+        for (i = 0; i < ARRAY_SIZE(fw_path); i++) {
+                struct file *file;
+                snprintf(path, PATH_MAX, "%s/%s", fw_path[i], buf->fw_id);
+
+                file = filp_open(path, O_RDONLY, 0);
+                if (IS_ERR(file))
+                        continue;
+                success = fw_read_file_contents(file, buf);
+                fput(file);
+                if (success)
+                        break;
+        }
+        __putname(path);
+        return success;
+}
+
 static struct firmware_priv *to_firmware_priv(struct device *dev)
 {
         return container_of(dev, struct firmware_priv, dev);
@@ -423,6 +433,21 @@ static void firmware_free_data(const struct firmware *fw)
 #ifndef PAGE_KERNEL_RO
 #define PAGE_KERNEL_RO PAGE_KERNEL
 #endif
+
+/* one pages buffer should be mapped/unmapped only once */
+static int fw_map_pages_buf(struct firmware_buf *buf)
+{
+        if (buf->fmt != PAGE_BUF)
+                return 0;
+
+        if (buf->data)
+                vunmap(buf->data);
+        buf->data = vmap(buf->pages, buf->nr_pages, 0, PAGE_KERNEL_RO);
+        if (!buf->data)
+                return -ENOMEM;
+        return 0;
+}
+
 /**
  * firmware_loading_store - set value in the 'loading' control file
  * @dev: device pointer
@@ -467,6 +492,14 @@ static ssize_t firmware_loading_store(struct device *dev,
                 if (test_bit(FW_STATUS_LOADING, &fw_buf->status)) {
                         set_bit(FW_STATUS_DONE, &fw_buf->status);
                         clear_bit(FW_STATUS_LOADING, &fw_buf->status);
+
+                        /*
+                         * Several loading requests may be pending on
+                         * one same firmware buf, so let all requests
+                         * see the mapped 'buf->data' once the loading
+                         * is completed.
+                         * */
+                        fw_map_pages_buf(fw_buf);
                         complete_all(&fw_buf->completion);
                         break;
                 }
@@ -670,15 +703,6 @@ exit:
         return fw_priv;
 }
 
-/* one pages buffer is mapped/unmapped only once */
-static int fw_map_pages_buf(struct firmware_buf *buf)
-{
-        buf->data = vmap(buf->pages, buf->nr_pages, 0, PAGE_KERNEL_RO);
-        if (!buf->data)
-                return -ENOMEM;
-        return 0;
-}
-
 /* store the pages buffer info firmware from buf */
 static void fw_set_page_data(struct firmware_buf *buf, struct firmware *fw)
 {
@@ -778,11 +802,6 @@ _request_firmware_prepare(const struct firmware **firmware_p, const char *name,
                 return NULL;
         }
 
-        if (fw_get_filesystem_firmware(firmware, name)) {
-                dev_dbg(device, "firmware: direct-loading firmware %s\n", name);
-                return NULL;
-        }
-
         ret = fw_lookup_and_allocate_buf(name, &fw_cache, &buf);
         if (!ret)
                 fw_priv = fw_create_instance(firmware, name, device,
@@ -832,6 +851,21 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
         struct device *f_dev = &fw_priv->dev;
         struct firmware_buf *buf = fw_priv->buf;
         struct firmware_cache *fwc = &fw_cache;
+        int direct_load = 0;
+
+        /* try direct loading from fs first */
+        if (fw_get_filesystem_firmware(buf)) {
+                dev_dbg(f_dev->parent, "firmware: direct-loading"
+                        " firmware %s\n", buf->fw_id);
+
+                set_bit(FW_STATUS_DONE, &buf->status);
+                complete_all(&buf->completion);
+                direct_load = 1;
+                goto handle_fw;
+        }
+
+        /* fall back on userspace loading */
+        buf->fmt = PAGE_BUF;
 
         dev_set_uevent_suppress(f_dev, true);
 
@@ -870,6 +904,7 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
 
         del_timer_sync(&fw_priv->timeout);
 
+handle_fw:
         mutex_lock(&fw_lock);
         if (!buf->size || test_bit(FW_STATUS_ABORT, &buf->status))
                 retval = -ENOENT;
@@ -884,9 +919,6 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
         if (!retval && f_dev->parent)
                 fw_add_devm_name(f_dev->parent, buf->fw_id);
 
-        if (!retval)
-                retval = fw_map_pages_buf(buf);
-
         /*
          * After caching firmware image is started, let it piggyback
          * on request firmware.
@@ -902,6 +934,9 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
         fw_priv->buf = NULL;
         mutex_unlock(&fw_lock);
 
+        if (direct_load)
+                goto err_put_dev;
+
         device_remove_file(f_dev, &dev_attr_loading);
 err_del_bin_attr:
         device_remove_bin_file(f_dev, &firmware_attr_data);
@@ -1129,6 +1164,8 @@ int uncache_firmware(const char *fw_name)
 }
 
 #ifdef CONFIG_PM_SLEEP
+static ASYNC_DOMAIN_EXCLUSIVE(fw_cache_domain);
+
 static struct fw_cache_entry *alloc_fw_cache_entry(const char *name)
 {
         struct fw_cache_entry *fce;
@@ -1142,17 +1179,27 @@ exit:
         return fce;
 }
 
-static int fw_cache_piggyback_on_request(const char *name)
+static int __fw_entry_found(const char *name)
 {
         struct firmware_cache *fwc = &fw_cache;
         struct fw_cache_entry *fce;
-        int ret = 0;
 
-        spin_lock(&fwc->name_lock);
         list_for_each_entry(fce, &fwc->fw_names, list) {
                 if (!strcmp(fce->name, name))
-                        goto found;
+                        return 1;
         }
+        return 0;
+}
+
+static int fw_cache_piggyback_on_request(const char *name)
+{
+        struct firmware_cache *fwc = &fw_cache;
+        struct fw_cache_entry *fce;
+        int ret = 0;
+
+        spin_lock(&fwc->name_lock);
+        if (__fw_entry_found(name))
+                goto found;
 
         fce = alloc_fw_cache_entry(name);
         if (fce) {
@@ -1185,12 +1232,6 @@ static void __async_dev_cache_fw_image(void *fw_entry,
 
                 free_fw_cache_entry(fce);
         }
-
-        spin_lock(&fwc->name_lock);
-        fwc->cnt--;
-        spin_unlock(&fwc->name_lock);
-
-        wake_up(&fwc->wait_queue);
 }
 
 /* called with dev->devres_lock held */
@@ -1229,11 +1270,19 @@ static void dev_cache_fw_image(struct device *dev, void *data)
                 list_del(&fce->list);
 
                 spin_lock(&fwc->name_lock);
-                fwc->cnt++;
-                list_add(&fce->list, &fwc->fw_names);
+                /* only one cache entry for one firmware */
+                if (!__fw_entry_found(fce->name)) {
+                        list_add(&fce->list, &fwc->fw_names);
+                } else {
+                        free_fw_cache_entry(fce);
+                        fce = NULL;
+                }
                 spin_unlock(&fwc->name_lock);
 
-                async_schedule(__async_dev_cache_fw_image, (void *)fce);
+                if (fce)
+                        async_schedule_domain(__async_dev_cache_fw_image,
+                                              (void *)fce,
+                                              &fw_cache_domain);
         }
 }
 
@@ -1275,6 +1324,9 @@ static void device_cache_fw_images(void)
 
         pr_debug("%s\n", __func__);
 
+        /* cancel uncache work */
+        cancel_delayed_work_sync(&fwc->work);
+
         /*
          * use small loading timeout for caching devices' firmware
          * because all these firmware images have been loaded
@@ -1292,21 +1344,7 @@ static void device_cache_fw_images(void)
         mutex_unlock(&fw_lock);
 
         /* wait for completion of caching firmware for all devices */
-        spin_lock(&fwc->name_lock);
-        for (;;) {
-                prepare_to_wait(&fwc->wait_queue, &wait,
-                                TASK_UNINTERRUPTIBLE);
-                if (!fwc->cnt)
-                        break;
-
-                spin_unlock(&fwc->name_lock);
-
-                schedule();
-
-                spin_lock(&fwc->name_lock);
-        }
-        spin_unlock(&fwc->name_lock);
-        finish_wait(&fwc->wait_queue, &wait);
+        async_synchronize_full_domain(&fw_cache_domain);
 
         loading_timeout = old_timeout;
 }
@@ -1394,9 +1432,7 @@ static void __init fw_cache_init(void)
 #ifdef CONFIG_PM_SLEEP
         spin_lock_init(&fw_cache.name_lock);
         INIT_LIST_HEAD(&fw_cache.fw_names);
-        fw_cache.cnt = 0;
 
-        init_waitqueue_head(&fw_cache.wait_queue);
         INIT_DELAYED_WORK(&fw_cache.work,
                           device_uncache_fw_images_work);
 
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index c22b869245d9..96b71b6536d6 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -1862,7 +1862,7 @@ int pm_genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
         cpuidle_drv = cpuidle_driver_ref();
         if (!cpuidle_drv) {
                 ret = -ENODEV;
-                goto out;
+                goto err_drv;
         }
         if (cpuidle_drv->state_count <= state) {
                 ret = -EINVAL;
@@ -1884,6 +1884,9 @@ int pm_genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
 
  err:
         cpuidle_driver_unref();
+
+ err_drv:
+        kfree(cpu_data);
         goto out;
 }
 
diff --git a/drivers/base/regmap/Kconfig b/drivers/base/regmap/Kconfig
index 6be390bd8bd1..f0d30543fcce 100644
--- a/drivers/base/regmap/Kconfig
+++ b/drivers/base/regmap/Kconfig
@@ -3,7 +3,7 @@
 # subsystems should select the appropriate symbols.
 
 config REGMAP
-        default y if (REGMAP_I2C || REGMAP_SPI)
+        default y if (REGMAP_I2C || REGMAP_SPI || REGMAP_MMIO || REGMAP_IRQ)
         select LZO_COMPRESS
         select LZO_DECOMPRESS
         select IRQ_DOMAIN if REGMAP_IRQ
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 432aeeedfd5e..d865470bc951 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -158,9 +158,10 @@ static int bcma_register_cores(struct bcma_bus *bus)
 
 static void bcma_unregister_cores(struct bcma_bus *bus)
 {
-        struct bcma_device *core;
+        struct bcma_device *core, *tmp;
 
-        list_for_each_entry(core, &bus->cores, list) {
+        list_for_each_entry_safe(core, tmp, &bus->cores, list) {
+                list_del(&core->list);
                 if (core->dev_registered)
                         device_unregister(&core->dev);
         }
diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
index f529407db93f..824e09c4d0d7 100644
--- a/drivers/block/Kconfig
+++ b/drivers/block/Kconfig
@@ -131,6 +131,7 @@ config BLK_CPQ_DA
 config BLK_CPQ_CISS_DA
         tristate "Compaq Smart Array 5xxx support"
         depends on PCI
+        select CHECK_SIGNATURE
         help
           This is the driver for Compaq Smart Array 5xxx controllers.
           Everyone using these boards should say Y here.
@@ -166,8 +167,8 @@ config BLK_DEV_DAC960
           module will be called DAC960.
 
 config BLK_DEV_UMEM
-        tristate "Micro Memory MM5415 Battery Backed RAM support (EXPERIMENTAL)"
-        depends on PCI && EXPERIMENTAL
+        tristate "Micro Memory MM5415 Battery Backed RAM support"
+        depends on PCI
         ---help---
           Saying Y here will include support for the MM5415 family of
           battery backed (Non-volatile) RAM cards.
@@ -430,8 +431,8 @@ config CDROM_PKTCDVD_BUFFERS
           a disc is opened for writing.
 
 config CDROM_PKTCDVD_WCACHE
-        bool "Enable write caching (EXPERIMENTAL)"
-        depends on CDROM_PKTCDVD && EXPERIMENTAL
+        bool "Enable write caching"
+        depends on CDROM_PKTCDVD
         help
           If enabled, write caching will be set for the CD-R/W device. For now
           this option is dangerous unless the CD-RW media is known good, as we
@@ -508,8 +509,8 @@ config XEN_BLKDEV_BACKEND
 
 
 config VIRTIO_BLK
-        tristate "Virtio block driver (EXPERIMENTAL)"
-        depends on EXPERIMENTAL && VIRTIO
+        tristate "Virtio block driver"
+        depends on VIRTIO
         ---help---
           This is the virtual block driver for virtio.  It can be used with
           lguest or QEMU based VMMs (like KVM or Xen).  Say Y or M.
@@ -528,7 +529,7 @@ config BLK_DEV_HD
 
 config BLK_DEV_RBD
         tristate "Rados block device (RBD)"
-        depends on INET && EXPERIMENTAL && BLOCK
+        depends on INET && BLOCK
         select CEPH_LIB
         select LIBCRC32C
         select CRYPTO_AES
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index b0f553b26d0f..ca83f96756ad 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -5205,7 +5205,6 @@ static void cciss_shutdown(struct pci_dev *pdev)
                 return;
         }
         /* write all data in the battery backed cache to disk */
-        memset(flush_buf, 0, 4);
         return_code = sendcmd_withirq(h, CCISS_CACHE_FLUSH, flush_buf,
                 4, 0, CTLR_LUNID, TYPE_CMD);
         kfree(flush_buf);
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index f93a0320e952..f55683ad4ffa 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -162,23 +162,12 @@ static const struct block_device_operations drbd_ops = {
         .release = drbd_release,
 };
 
-static void bio_destructor_drbd(struct bio *bio)
-{
-        bio_free(bio, drbd_md_io_bio_set);
-}
-
 struct bio *bio_alloc_drbd(gfp_t gfp_mask)
 {
-        struct bio *bio;
-
         if (!drbd_md_io_bio_set)
                 return bio_alloc(gfp_mask, 1);
 
-        bio = bio_alloc_bioset(gfp_mask, 1, drbd_md_io_bio_set);
-        if (!bio)
-                return NULL;
-        bio->bi_destructor = bio_destructor_drbd;
-        return bio;
+        return bio_alloc_bioset(gfp_mask, 1, drbd_md_io_bio_set);
 }
 
 #ifdef __CHECKER__
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 17c675c52295..1c49d7173966 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -4109,12 +4109,19 @@ static struct platform_driver floppy_driver = {
 
 static struct platform_device floppy_device[N_DRIVE];
 
+static bool floppy_available(int drive)
+{
+        if (!(allowed_drive_mask & (1 << drive)))
+                return false;
+        if (fdc_state[FDC(drive)].version == FDC_NONE)
+                return false;
+        return true;
+}
+
 static struct kobject *floppy_find(dev_t dev, int *part, void *data)
 {
         int drive = (*part & 3) | ((*part & 0x80) >> 5);
-        if (drive >= N_DRIVE ||
-            !(allowed_drive_mask & (1 << drive)) ||
-            fdc_state[FDC(drive)].version == FDC_NONE)
+        if (drive >= N_DRIVE || !floppy_available(drive))
                 return NULL;
         if (((*part >> 2) & 0x1f) >= ARRAY_SIZE(floppy_type))
                 return NULL;
@@ -4124,8 +4131,7 @@ static struct kobject *floppy_find(dev_t dev, int *part, void *data)
 
 static int __init do_floppy_init(void)
 {
-        int i, unit, drive;
-        int err, dr;
+        int i, unit, drive, err;
 
         set_debugt();
         interruptjiffies = resultjiffies = jiffies;
@@ -4137,34 +4143,32 @@ static int __init do_floppy_init(void)
 
         raw_cmd = NULL;
 
-        for (dr = 0; dr < N_DRIVE; dr++) {
-                disks[dr] = alloc_disk(1);
-                if (!disks[dr]) {
-                        err = -ENOMEM;
-                        goto out_put_disk;
-                }
+        floppy_wq = alloc_ordered_workqueue("floppy", 0);
+        if (!floppy_wq)
+                return -ENOMEM;
 
-                floppy_wq = alloc_ordered_workqueue("floppy", 0);
-                if (!floppy_wq) {
+        for (drive = 0; drive < N_DRIVE; drive++) {
+                disks[drive] = alloc_disk(1);
+                if (!disks[drive]) {
                         err = -ENOMEM;
                         goto out_put_disk;
                 }
 
-                disks[dr]->queue = blk_init_queue(do_fd_request, &floppy_lock);
-                if (!disks[dr]->queue) {
+                disks[drive]->queue = blk_init_queue(do_fd_request, &floppy_lock);
+                if (!disks[drive]->queue) {
                         err = -ENOMEM;
-                        goto out_destroy_workq;
+                        goto out_put_disk;
                 }
 
-                blk_queue_max_hw_sectors(disks[dr]->queue, 64);
-                disks[dr]->major = FLOPPY_MAJOR;
-                disks[dr]->first_minor = TOMINOR(dr);
-                disks[dr]->fops = &floppy_fops;
-                sprintf(disks[dr]->disk_name, "fd%d", dr);
+                blk_queue_max_hw_sectors(disks[drive]->queue, 64);
+                disks[drive]->major = FLOPPY_MAJOR;
+                disks[drive]->first_minor = TOMINOR(drive);
+                disks[drive]->fops = &floppy_fops;
+                sprintf(disks[drive]->disk_name, "fd%d", drive);
 
-                init_timer(&motor_off_timer[dr]);
-                motor_off_timer[dr].data = dr;
-                motor_off_timer[dr].function = motor_off_callback;
+                init_timer(&motor_off_timer[drive]);
+                motor_off_timer[drive].data = drive;
+                motor_off_timer[drive].function = motor_off_callback;
         }
 
         err = register_blkdev(FLOPPY_MAJOR, "fd");
@@ -4282,9 +4286,7 @@ static int __init do_floppy_init(void)
         }
 
         for (drive = 0; drive < N_DRIVE; drive++) {
-                if (!(allowed_drive_mask & (1 << drive)))
-                        continue;
-                if (fdc_state[FDC(drive)].version == FDC_NONE)
+                if (!floppy_available(drive))
                         continue;
 
                 floppy_device[drive].name = floppy_device_name;
@@ -4293,7 +4295,7 @@ static int __init do_floppy_init(void)
 
                 err = platform_device_register(&floppy_device[drive]);
                 if (err)
-                        goto out_release_dma;
+                        goto out_remove_drives;
 
                 err = device_create_file(&floppy_device[drive].dev,
                                          &dev_attr_cmos);
@@ -4311,29 +4313,34 @@ static int __init do_floppy_init(void)
 
 out_unreg_platform_dev:
         platform_device_unregister(&floppy_device[drive]);
+out_remove_drives:
+        while (drive--) {
+                if (floppy_available(drive)) {
+                        del_gendisk(disks[drive]);
+                        device_remove_file(&floppy_device[drive].dev, &dev_attr_cmos);
+                        platform_device_unregister(&floppy_device[drive]);
+                }
+        }
 out_release_dma:
         if (atomic_read(&usage_count))
                 floppy_release_irq_and_dma();
 out_unreg_region:
         blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
         platform_driver_unregister(&floppy_driver);
-out_destroy_workq:
-        destroy_workqueue(floppy_wq);
 out_unreg_blkdev:
         unregister_blkdev(FLOPPY_MAJOR, "fd");
 out_put_disk:
-        while (dr--) {
-                del_timer_sync(&motor_off_timer[dr]);
-                if (disks[dr]->queue) {
-                        blk_cleanup_queue(disks[dr]->queue);
-                        /*
-                         * put_disk() is not paired with add_disk() and
-                         * will put queue reference one extra time. fix it.
-                         */
-                        disks[dr]->queue = NULL;
+        for (drive = 0; drive < N_DRIVE; drive++) {
+                if (!disks[drive])
+                        break;
+                if (disks[drive]->queue) {
+                        del_timer_sync(&motor_off_timer[drive]);
+                        blk_cleanup_queue(disks[drive]->queue);
+                        disks[drive]->queue = NULL;
                 }
-                put_disk(disks[dr]);
+                put_disk(disks[drive]);
         }
+        destroy_workqueue(floppy_wq);
         return err;
 }
 
@@ -4551,8 +4558,7 @@ static void __exit floppy_module_exit(void)
         for (drive = 0; drive < N_DRIVE; drive++) {
                 del_timer_sync(&motor_off_timer[drive]);
 
-                if ((allowed_drive_mask & (1 << drive)) &&
-                    fdc_state[FDC(drive)].version != FDC_NONE) {
+                if (floppy_available(drive)) {
                         del_gendisk(disks[drive]);
                         device_remove_file(&floppy_device[drive].dev, &dev_attr_cmos);
                         platform_device_unregister(&floppy_device[drive]);
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index e9d594fd12cb..54046e51160a 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -976,8 +976,21 @@ static int loop_clr_fd(struct loop_device *lo)
         if (lo->lo_state != Lo_bound)
                 return -ENXIO;
 
-        if (lo->lo_refcnt > 1)  /* we needed one fd for the ioctl */
-                return -EBUSY;
+        /*
+         * If we've explicitly asked to tear down the loop device,
+         * and it has an elevated reference count, set it for auto-teardown when
+         * the last reference goes away. This stops $!~#$@ udev from
+         * preventing teardown because it decided that it needs to run blkid on
+         * the loopback device whenever they appear. xfstests is notorious for
+         * failing tests because blkid via udev races with a losetup
+         * <dev>/do something like mkfs/losetup -d <dev> causing the losetup -d
+         * command to fail with EBUSY.
+         */
+        if (lo->lo_refcnt > 1) {
+                lo->lo_flags |= LO_FLAGS_AUTOCLEAR;
+                mutex_unlock(&lo->lo_ctl_mutex);
+                return 0;
+        }
 
         if (filp == NULL)
                 return -EINVAL;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index f946d31d6917..adc6f36564cf 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -2035,8 +2035,9 @@ static unsigned int implicit_sector(unsigned char command,
         }
         return rv;
 }
-
-static void mtip_set_timeout(struct host_to_dev_fis *fis, unsigned int *timeout)
+static void mtip_set_timeout(struct driver_data *dd,
+                                        struct host_to_dev_fis *fis,
+                                        unsigned int *timeout, u8 erasemode)
 {
         switch (fis->command) {
         case ATA_CMD_DOWNLOAD_MICRO:
@@ -2044,7 +2045,10 @@ static void mtip_set_timeout(struct host_to_dev_fis *fis, unsigned int *timeout)
                 break;
         case ATA_CMD_SEC_ERASE_UNIT:
         case 0xFC:
-                *timeout = 240000; /* 4 minutes */
+                if (erasemode)
+                        *timeout = ((*(dd->port->identify + 90) * 2) * 60000);
+                else
+                        *timeout = ((*(dd->port->identify + 89) * 2) * 60000);
                 break;
         case ATA_CMD_STANDBYNOW1:
                 *timeout = 120000;  /* 2 minutes */
@@ -2087,6 +2091,7 @@ static int exec_drive_taskfile(struct driver_data *dd,
         unsigned int transfer_size;
         unsigned long task_file_data;
         int intotal = outtotal + req_task->out_size;
+        int erasemode = 0;
 
         taskout = req_task->out_size;
         taskin = req_task->in_size;
@@ -2212,7 +2217,13 @@ static int exec_drive_taskfile(struct driver_data *dd,
                 fis.lba_hi,
                 fis.device);
 
-        mtip_set_timeout(&fis, &timeout);
+        /* check for erase mode support during secure erase.*/
+        if ((fis.command == ATA_CMD_SEC_ERASE_UNIT)
+                                        && (outbuf[0] & MTIP_SEC_ERASE_MODE)) {
+                erasemode = 1;
+        }
+
+        mtip_set_timeout(dd, &fis, &timeout, erasemode);
 
         /* Determine the correct transfer size.*/
         if (force_single_sector)
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index 18627a1d04c5..5f4a917bd8bb 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -33,6 +33,9 @@
 /* offset of Device Control register in PCIe extended capabilites space */
 #define PCIE_CONFIG_EXT_DEVICE_CONTROL_OFFSET   0x48
 
+/* check for erase mode support during secure erase */
+#define MTIP_SEC_ERASE_MODE     0x3
+
 /* # of times to retry timed out/failed IOs */
 #define MTIP_MAX_RETRIES        2
 
diff --git a/drivers/block/osdblk.c b/drivers/block/osdblk.c
index 87311ebac0db..1bbc681688e4 100644
--- a/drivers/block/osdblk.c
+++ b/drivers/block/osdblk.c
@@ -266,11 +266,10 @@ static struct bio *bio_chain_clone(struct bio *old_chain, gfp_t gfpmask)
         struct bio *tmp, *new_chain = NULL, *tail = NULL;
 
         while (old_chain) {
-                tmp = bio_kmalloc(gfpmask, old_chain->bi_max_vecs);
+                tmp = bio_clone_kmalloc(old_chain, gfpmask);
                 if (!tmp)
                         goto err_out;
 
-                __bio_clone(tmp, old_chain);
                 tmp->bi_bdev = NULL;
                 gfpmask &= ~__GFP_WAIT;
                 tmp->bi_next = NULL;
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index ba66e4445f41..2e7de7a59bfc 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -522,38 +522,6 @@ static void pkt_bio_finished(struct pktcdvd_device *pd)
         }
 }
 
-static void pkt_bio_destructor(struct bio *bio)
-{
-        kfree(bio->bi_io_vec);
-        kfree(bio);
-}
-
-static struct bio *pkt_bio_alloc(int nr_iovecs)
-{
-        struct bio_vec *bvl = NULL;
-        struct bio *bio;
-
-        bio = kmalloc(sizeof(struct bio), GFP_KERNEL);
-        if (!bio)
-                goto no_bio;
-        bio_init(bio);
-
-        bvl = kcalloc(nr_iovecs, sizeof(struct bio_vec), GFP_KERNEL);
-        if (!bvl)
-                goto no_bvl;
-
-        bio->bi_max_vecs = nr_iovecs;
-        bio->bi_io_vec = bvl;
-        bio->bi_destructor = pkt_bio_destructor;
-
-        return bio;
-
- no_bvl:
-        kfree(bio);
- no_bio:
-        return NULL;
-}
-
 /*
  * Allocate a packet_data struct
  */
@@ -567,7 +535,7 @@ static struct packet_data *pkt_alloc_packet_data(int frames)
                 goto no_pkt;
 
         pkt->frames = frames;
-        pkt->w_bio = pkt_bio_alloc(frames);
+        pkt->w_bio = bio_kmalloc(GFP_KERNEL, frames);
         if (!pkt->w_bio)
                 goto no_bio;
 
@@ -581,9 +549,10 @@ static struct packet_data *pkt_alloc_packet_data(int frames)
         bio_list_init(&pkt->orig_bios);
 
         for (i = 0; i < frames; i++) {
-                struct bio *bio = pkt_bio_alloc(1);
+                struct bio *bio = bio_kmalloc(GFP_KERNEL, 1);
                 if (!bio)
                         goto no_rd_bio;
+
                 pkt->r_bios[i] = bio;
         }
 
@@ -1111,21 +1080,17 @@ static void pkt_gather_data(struct pktcdvd_device *pd, struct packet_data *pkt)
          * Schedule reads for missing parts of the packet.
          */
         for (f = 0; f < pkt->frames; f++) {
-                struct bio_vec *vec;
-
                 int p, offset;
+
                 if (written[f])
                         continue;
+
                 bio = pkt->r_bios[f];
-                vec = bio->bi_io_vec;
-                bio_init(bio);
-                bio->bi_max_vecs = 1;
+                bio_reset(bio);
                 bio->bi_sector = pkt->sector + f * (CD_FRAMESIZE >> 9);
                 bio->bi_bdev = pd->bdev;
                 bio->bi_end_io = pkt_end_io_read;
                 bio->bi_private = pkt;
-                bio->bi_io_vec = vec;
-                bio->bi_destructor = pkt_bio_destructor;
 
                 p = (f * CD_FRAMESIZE) / PAGE_SIZE;
                 offset = (f * CD_FRAMESIZE) % PAGE_SIZE;
@@ -1418,14 +1383,11 @@ static void pkt_start_write(struct pktcdvd_device *pd, struct packet_data *pkt)
         }
 
         /* Start the write request */
-        bio_init(pkt->w_bio);
-        pkt->w_bio->bi_max_vecs = PACKET_MAX_SIZE;
+        bio_reset(pkt->w_bio);
         pkt->w_bio->bi_sector = pkt->sector;
         pkt->w_bio->bi_bdev = pd->bdev;
         pkt->w_bio->bi_end_io = pkt_end_io_packet_write;
         pkt->w_bio->bi_private = pkt;
-        pkt->w_bio->bi_io_vec = bvec;
-        pkt->w_bio->bi_destructor = pkt_bio_destructor;
         for (f = 0; f < pkt->frames; f++)
                 if (!bio_add_page(pkt->w_bio, bvec[f].bv_page, CD_FRAMESIZE, bvec[f].bv_offset))
                         BUG();
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index 9ad3b5ec1dc1..9a54623e52d7 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -158,8 +158,8 @@ struct xen_vbd {
         struct block_device     *bdev;
         /* Cached size parameter. */
         sector_t                size;
-        bool                    flush_support;
-        bool                    discard_secure;
+        unsigned int            flush_support:1;
+        unsigned int            discard_secure:1;
 };
 
 struct backend_info;
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 4f66171c6683..f58434c2617c 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -105,11 +105,10 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 {
         struct xen_blkif *blkif;
 
-        blkif = kmem_cache_alloc(xen_blkif_cachep, GFP_KERNEL);
+        blkif = kmem_cache_zalloc(xen_blkif_cachep, GFP_KERNEL);
         if (!blkif)
                 return ERR_PTR(-ENOMEM);
 
-        memset(blkif, 0, sizeof(*blkif));
         blkif->domid = domid;
         spin_lock_init(&blkif->blk_ring_lock);
         atomic_set(&blkif->refcnt, 1);
@@ -196,7 +195,7 @@ static void xen_blkif_disconnect(struct xen_blkif *blkif)
         }
 }
 
-void xen_blkif_free(struct xen_blkif *blkif)
+static void xen_blkif_free(struct xen_blkif *blkif)
 {
         if (!atomic_dec_and_test(&blkif->refcnt))
                 BUG();
@@ -257,7 +256,7 @@ static struct attribute_group xen_vbdstat_group = {
 VBD_SHOW(physical_device, "%x:%x\n", be->major, be->minor);
 VBD_SHOW(mode, "%s\n", be->mode);
 
-int xenvbd_sysfs_addif(struct xenbus_device *dev)
+static int xenvbd_sysfs_addif(struct xenbus_device *dev)
 {
         int error;
 
@@ -281,7 +280,7 @@ fail1:	device_remove_file(&dev->dev, &dev_attr_physical_device);
         return error;
 }
 
-void xenvbd_sysfs_delif(struct xenbus_device *dev)
+static void xenvbd_sysfs_delif(struct xenbus_device *dev)
 {
         sysfs_remove_group(&dev->dev.kobj, &xen_vbdstat_group);
         device_remove_file(&dev->dev, &dev_attr_mode);
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index d0b27a39f1d4..7ff1d0d208a7 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -52,7 +52,6 @@ obj-$(CONFIG_TELCLOCK)		+= tlclk.o
 obj-$(CONFIG_MWAVE)             += mwave/
 obj-$(CONFIG_AGP)               += agp/
 obj-$(CONFIG_PCMCIA)            += pcmcia/
-obj-$(CONFIG_IPMI_HANDLER)      += ipmi/
 
 obj-$(CONFIG_HANGCHECK_TIMER)   += hangcheck-timer.o
 obj-$(CONFIG_TCG_TPM)           += tpm/
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index e01f5eaaec82..38390f7c6ab6 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -667,7 +667,7 @@ static int intel_gtt_init(void)
         gtt_map_size = intel_private.base.gtt_total_entries * 4;
 
         intel_private.gtt = NULL;
-        if (INTEL_GTT_GEN < 6)
+        if (INTEL_GTT_GEN < 6 && INTEL_GTT_GEN > 2)
                 intel_private.gtt = ioremap_wc(intel_private.gtt_bus_addr,
                                                gtt_map_size);
         if (intel_private.gtt == NULL)
diff --git a/drivers/char/ds1620.c b/drivers/char/ds1620.c
index aab9605f0b43..24ffd8cec51e 100644
--- a/drivers/char/ds1620.c
+++ b/drivers/char/ds1620.c
@@ -74,21 +74,21 @@ static inline void netwinder_ds1620_reset(void)
 
 static inline void netwinder_lock(unsigned long *flags)
 {
-        spin_lock_irqsave(&nw_gpio_lock, *flags);
+        raw_spin_lock_irqsave(&nw_gpio_lock, *flags);
 }
 
 static inline void netwinder_unlock(unsigned long *flags)
 {
-        spin_unlock_irqrestore(&nw_gpio_lock, *flags);
+        raw_spin_unlock_irqrestore(&nw_gpio_lock, *flags);
 }
 
 static inline void netwinder_set_fan(int i)
 {
         unsigned long flags;
 
-        spin_lock_irqsave(&nw_gpio_lock, flags);
+        raw_spin_lock_irqsave(&nw_gpio_lock, flags);
         nw_gpio_modify_op(GPIO_FAN, i ? GPIO_FAN : 0);
-        spin_unlock_irqrestore(&nw_gpio_lock, flags);
+        raw_spin_unlock_irqrestore(&nw_gpio_lock, flags);
 }
 
 static inline int netwinder_get_fan(void)
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index 2c29942b1326..a0c84bb30856 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -1880,7 +1880,7 @@ int ipmi_request_supply_msgs(ipmi_user_t          user,
                              struct ipmi_recv_msg *supplied_recv,
                              int                  priority)
 {
-        unsigned char saddr, lun;
+        unsigned char saddr = 0, lun = 0;
         int           rv;
 
         if (!user)
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 83f85cf7fb1b..32a6c7e256bd 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -2424,6 +2424,38 @@ static void ipmi_pci_cleanup(struct smi_info *info)
         pci_disable_device(pdev);
 }
 
+static int __devinit ipmi_pci_probe_regspacing(struct smi_info *info)
+{
+        if (info->si_type == SI_KCS) {
+                unsigned char   status;
+                int             regspacing;
+
+                info->io.regsize = DEFAULT_REGSIZE;
+                info->io.regshift = 0;
+                info->io_size = 2;
+                info->handlers = &kcs_smi_handlers;
+
+                /* detect 1, 4, 16byte spacing */
+                for (regspacing = DEFAULT_REGSPACING; regspacing <= 16;) {
+                        info->io.regspacing = regspacing;
+                        if (info->io_setup(info)) {
+                                dev_err(info->dev,
+                                        "Could not setup I/O space\n");
+                                return DEFAULT_REGSPACING;
+                        }
+                        /* write invalid cmd */
+                        info->io.outputb(&info->io, 1, 0x10);
+                        /* read status back */
+                        status = info->io.inputb(&info->io, 1);
+                        info->io_cleanup(info);
+                        if (status)
+                                return regspacing;
+                        regspacing *= 4;
+                }
+        }
+        return DEFAULT_REGSPACING;
+}
+
 static int __devinit ipmi_pci_probe(struct pci_dev *pdev,
                                     const struct pci_device_id *ent)
 {
@@ -2476,8 +2508,8 @@ static int __devinit ipmi_pci_probe(struct pci_dev *pdev,
         }
         info->io.addr_data = pci_resource_start(pdev, 0);
 
-        info->io.regspacing = DEFAULT_REGSPACING;
-        info->io.regsize = DEFAULT_REGSPACING;
+        info->io.regspacing = ipmi_pci_probe_regspacing(info);
+        info->io.regsize = DEFAULT_REGSIZE;
         info->io.regshift = 0;
 
         info->irq = pdev->irq;
diff --git a/drivers/char/nwflash.c b/drivers/char/nwflash.c
index a0e2f7d70355..e371480d3639 100644
--- a/drivers/char/nwflash.c
+++ b/drivers/char/nwflash.c
@@ -583,9 +583,9 @@ static void kick_open(void)
          * we want to write a bit pattern XXX1 to Xilinx to enable
          * the write gate, which will be open for about the next 2ms.
          */
-        spin_lock_irqsave(&nw_gpio_lock, flags);
+        raw_spin_lock_irqsave(&nw_gpio_lock, flags);
         nw_cpld_modify(CPLD_FLASH_WR_ENABLE, CPLD_FLASH_WR_ENABLE);
-        spin_unlock_irqrestore(&nw_gpio_lock, flags);
+        raw_spin_unlock_irqrestore(&nw_gpio_lock, flags);
 
         /*
          * let the ISA bus to catch on...
diff --git a/drivers/char/raw.c b/drivers/char/raw.c
index 54a3a6d09819..0bb207eaef2f 100644
--- a/drivers/char/raw.c
+++ b/drivers/char/raw.c
@@ -285,7 +285,7 @@ static long raw_ctl_compat_ioctl(struct file *file, unsigned int cmd,
 
 static const struct file_operations raw_fops = {
         .read           = do_sync_read,
-        .aio_read       = generic_file_aio_read,
+        .aio_read       = blkdev_aio_read,
         .write          = do_sync_write,
         .aio_write      = blkdev_aio_write,
         .fsync          = blkdev_fsync,
diff --git a/drivers/char/sonypi.c b/drivers/char/sonypi.c
index 320debbe32fa..9b4f0116ff21 100644
--- a/drivers/char/sonypi.c
+++ b/drivers/char/sonypi.c
@@ -1456,7 +1456,7 @@ static int __devexit sonypi_remove(struct platform_device *dev)
         return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int old_camera_power;
 
 static int sonypi_suspend(struct device *dev)
diff --git a/drivers/char/tpm/tpm.c b/drivers/char/tpm/tpm.c
index f26afdb1a702..93211df52aab 100644
--- a/drivers/char/tpm/tpm.c
+++ b/drivers/char/tpm/tpm.c
@@ -1182,17 +1182,20 @@ ssize_t tpm_write(struct file *file, const char __user *buf,
                   size_t size, loff_t *off)
 {
         struct tpm_chip *chip = file->private_data;
-        size_t in_size = size, out_size;
+        size_t in_size = size;
+        ssize_t out_size;
 
         /* cannot perform a write until the read has cleared
-           either via tpm_read or a user_read_timer timeout */
-        while (atomic_read(&chip->data_pending) != 0)
-                msleep(TPM_TIMEOUT);
-
-        mutex_lock(&chip->buffer_mutex);
+           either via tpm_read or a user_read_timer timeout.
+           This also prevents splitted buffered writes from blocking here.
+        */
+        if (atomic_read(&chip->data_pending) != 0)
+                return -EBUSY;
 
         if (in_size > TPM_BUFSIZE)
-                in_size = TPM_BUFSIZE;
+                return -E2BIG;
+
+        mutex_lock(&chip->buffer_mutex);
 
         if (copy_from_user
             (chip->data_buffer, (void __user *) buf, in_size)) {
@@ -1202,6 +1205,10 @@ ssize_t tpm_write(struct file *file, const char __user *buf,
 
         /* atomic tpm command send and result receive */
         out_size = tpm_transmit(chip, chip->data_buffer, TPM_BUFSIZE);
+        if (out_size < 0) {
+                mutex_unlock(&chip->buffer_mutex);
+                return out_size;
+        }
 
         atomic_set(&chip->data_pending, out_size);
         mutex_unlock(&chip->buffer_mutex);
@@ -1259,6 +1266,7 @@ void tpm_remove_hardware(struct device *dev)
 
         misc_deregister(&chip->vendor.miscdev);
         sysfs_remove_group(&dev->kobj, chip->vendor.attr_group);
+        tpm_remove_ppi(&dev->kobj);
         tpm_bios_log_teardown(chip->bios_dir);
 
         /* write it this way to be explicit (chip->dev == dev) */
@@ -1476,7 +1484,7 @@ struct tpm_chip *tpm_register_hardware(struct device *dev,
                 goto put_device;
         }
 
-        if (sys_add_ppi(&dev->kobj)) {
+        if (tpm_add_ppi(&dev->kobj)) {
                 misc_deregister(&chip->vendor.miscdev);
                 goto put_device;
         }
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index 02c266aa2bf7..8ef7649a50aa 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -329,10 +329,15 @@ extern int wait_for_tpm_stat(struct tpm_chip *, u8, unsigned long,
                              wait_queue_head_t *);
 
 #ifdef CONFIG_ACPI
-extern ssize_t sys_add_ppi(struct kobject *parent);
+extern int tpm_add_ppi(struct kobject *);
+extern void tpm_remove_ppi(struct kobject *);
 #else
-static inline ssize_t sys_add_ppi(struct kobject *parent)
+static inline int tpm_add_ppi(struct kobject *parent)
 {
         return 0;
 }
+
+static inline void tpm_remove_ppi(struct kobject *parent)
+{
+}
 #endif
diff --git a/drivers/char/tpm/tpm_ppi.c b/drivers/char/tpm/tpm_ppi.c
index f27b58cfae98..720ebcf29fdf 100644
--- a/drivers/char/tpm/tpm_ppi.c
+++ b/drivers/char/tpm/tpm_ppi.c
@@ -444,18 +444,20 @@ static struct attribute *ppi_attrs[] = {
         &dev_attr_vs_operations.attr, NULL,
 };
 static struct attribute_group ppi_attr_grp = {
+        .name = "ppi",
         .attrs = ppi_attrs
 };
 
-ssize_t sys_add_ppi(struct kobject *parent)
+int tpm_add_ppi(struct kobject *parent)
 {
-        struct kobject *ppi;
-        ppi = kobject_create_and_add("ppi", parent);
-        if (sysfs_create_group(ppi, &ppi_attr_grp))
-                return -EFAULT;
-        else
-                return 0;
+        return sysfs_create_group(parent, &ppi_attr_grp);
+}
+EXPORT_SYMBOL_GPL(tpm_add_ppi);
+
+void tpm_remove_ppi(struct kobject *parent)
+{
+        sysfs_remove_group(parent, &ppi_attr_grp);
 }
-EXPORT_SYMBOL_GPL(sys_add_ppi);
+EXPORT_SYMBOL_GPL(tpm_remove_ppi);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/cpufreq_stats.c b/drivers/cpufreq/cpufreq_stats.c
index b40ee1403be9..399831690fed 100644
--- a/drivers/cpufreq/cpufreq_stats.c
+++ b/drivers/cpufreq/cpufreq_stats.c
@@ -328,6 +328,7 @@ static int __cpuinit cpufreq_stat_cpu_callback(struct notifier_block *nfb,
                 cpufreq_update_policy(cpu);
                 break;
         case CPU_DOWN_PREPARE:
+        case CPU_DOWN_PREPARE_FROZEN:
                 cpufreq_stats_free_sysfs(cpu);
                 break;
         case CPU_DEAD:
diff --git a/drivers/cpufreq/omap-cpufreq.c b/drivers/cpufreq/omap-cpufreq.c
index 65f8e9a54975..1f3417a8322d 100644
--- a/drivers/cpufreq/omap-cpufreq.c
+++ b/drivers/cpufreq/omap-cpufreq.c
@@ -30,20 +30,12 @@
 #include <asm/smp_plat.h>
 #include <asm/cpu.h>
 
-#include <plat/clock.h>
-#include <plat/omap-pm.h>
-#include <plat/common.h>
-#include <plat/omap_device.h>
-
-#include <mach/hardware.h>
-
 /* OPP tolerance in percentage */
 #define OPP_TOLERANCE   4
 
 static struct cpufreq_frequency_table *freq_table;
 static atomic_t freq_table_users = ATOMIC_INIT(0);
 static struct clk *mpu_clk;
-static char *mpu_clk_name;
 static struct device *mpu_dev;
 static struct regulator *mpu_reg;
 
@@ -108,6 +100,14 @@ static int omap_target(struct cpufreq_policy *policy,
         }
 
         freq = freqs.new * 1000;
+        ret = clk_round_rate(mpu_clk, freq);
+        if (IS_ERR_VALUE(ret)) {
+                dev_warn(mpu_dev,
+                         "CPUfreq: Cannot find matching frequency for %lu\n",
+                         freq);
+                return ret;
+        }
+        freq = ret;
 
         if (mpu_reg) {
                 opp = opp_find_freq_ceil(mpu_dev, &freq);
@@ -172,7 +172,7 @@ static int __cpuinit omap_cpu_init(struct cpufreq_policy *policy)
 {
         int result = 0;
 
-        mpu_clk = clk_get(NULL, mpu_clk_name);
+        mpu_clk = clk_get(NULL, "cpufreq_ck");
         if (IS_ERR(mpu_clk))
                 return PTR_ERR(mpu_clk);
 
@@ -253,22 +253,10 @@ static struct cpufreq_driver omap_driver = {
 
 static int __init omap_cpufreq_init(void)
 {
-        if (cpu_is_omap24xx())
-                mpu_clk_name = "virt_prcm_set";
-        else if (cpu_is_omap34xx())
-                mpu_clk_name = "dpll1_ck";
-        else if (cpu_is_omap44xx())
-                mpu_clk_name = "dpll_mpu_ck";
-
-        if (!mpu_clk_name) {
-                pr_err("%s: unsupported Silicon?\n", __func__);
-                return -EINVAL;
-        }
-
-        mpu_dev = omap_device_get_by_hwmod_name("mpu");
-        if (IS_ERR(mpu_dev)) {
+        mpu_dev = get_cpu_device(0);
+        if (!mpu_dev) {
                 pr_warning("%s: unable to get the mpu device\n", __func__);
-                return PTR_ERR(mpu_dev);
+                return -EINVAL;
         }
 
         mpu_reg = regulator_get(mpu_dev, "vcc");
diff --git a/drivers/cpufreq/powernow-k8.c b/drivers/cpufreq/powernow-k8.c
index 129e80bfff22..e3ebb4fa2c3e 100644
--- a/drivers/cpufreq/powernow-k8.c
+++ b/drivers/cpufreq/powernow-k8.c
@@ -5,7 +5,7 @@
  *  http://www.gnu.org/licenses/gpl.html
  *
  *  Maintainer:
- *  Andreas Herrmann <andreas.herrmann3@amd.com>
+ *  Andreas Herrmann <herrmann.der.user@googlemail.com>
  *
  *  Based on the powernow-k7.c module written by Dave Jones.
  *  (C) 2003 Dave Jones on behalf of SuSE Labs
@@ -1052,14 +1052,7 @@ static int powernowk8_target(struct cpufreq_policy *pol,
         struct powernowk8_target_arg pta = { .pol = pol, .targfreq = targfreq,
                                              .relation = relation };
 
-        /*
-         * Must run on @pol->cpu.  cpufreq core is responsible for ensuring
-         * that we're bound to the current CPU and pol->cpu stays online.
-         */
-        if (smp_processor_id() == pol->cpu)
-                return powernowk8_target_fn(&pta);
-        else
-                return work_on_cpu(pol->cpu, powernowk8_target_fn, &pta);
+        return work_on_cpu(pol->cpu, powernowk8_target_fn, &pta);
 }
 
 /* Driver entry point to verify the policy and range of frequencies */
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index e28f6ea46f1a..7f15b8514a18 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -368,7 +368,7 @@ EXPORT_SYMBOL_GPL(cpuidle_enable_device);
  */
 void cpuidle_disable_device(struct cpuidle_device *dev)
 {
-        if (!dev->enabled)
+        if (!dev || !dev->enabled)
                 return;
         if (!cpuidle_get_driver() || !cpuidle_curr_governor)
                 return;
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index d06ea2950dd9..d4c12180c654 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -90,6 +90,17 @@ config DW_DMAC
           Support the Synopsys DesignWare AHB DMA controller.  This
           can be integrated in chips such as the Atmel AT32ap7000.
 
+config DW_DMAC_BIG_ENDIAN_IO
+        bool "Use big endian I/O register access"
+        default y if AVR32
+        depends on DW_DMAC
+        help
+          Say yes here to use big endian I/O access when reading and writing
+          to the DMA controller registers. This is needed on some platforms,
+          like the Atmel AVR32 architecture.
+
+          If unsure, use the default setting.
+
 config AT_HDMAC
         tristate "Atmel AHB DMA support"
         depends on ARCH_AT91
@@ -208,6 +219,16 @@ config SIRF_DMA
         help
           Enable support for the CSR SiRFprimaII DMA engine.
 
+config TI_EDMA
+        tristate "TI EDMA support"
+        depends on ARCH_DAVINCI
+        select DMA_ENGINE
+        select DMA_VIRTUAL_CHANNELS
+        default n
+        help
+          Enable support for the TI EDMA controller. This DMA
+          engine is found on TI DaVinci and AM33xx parts.
+
 config ARCH_HAS_ASYNC_TX_FIND_CHANNEL
         bool
 
@@ -292,6 +313,13 @@ config DMA_OMAP
         select DMA_ENGINE
         select DMA_VIRTUAL_CHANNELS
 
+config MMP_PDMA
+        bool "MMP PDMA support"
+        depends on (ARCH_MMP || ARCH_PXA)
+        select DMA_ENGINE
+        help
+          Support the MMP PDMA engine for PXA and MMP platfrom.
+
 config DMA_ENGINE
         bool
 
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 4cf6b128ab9a..7428feaa8705 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -23,6 +23,7 @@ obj-$(CONFIG_IMX_DMA) += imx-dma.o
 obj-$(CONFIG_MXS_DMA) += mxs-dma.o
 obj-$(CONFIG_TIMB_DMA) += timb_dma.o
 obj-$(CONFIG_SIRF_DMA) += sirf-dma.o
+obj-$(CONFIG_TI_EDMA) += edma.o
 obj-$(CONFIG_STE_DMA40) += ste_dma40.o ste_dma40_ll.o
 obj-$(CONFIG_TEGRA20_APB_DMA) += tegra20-apb-dma.o
 obj-$(CONFIG_PL330_DMA) += pl330.o
@@ -32,3 +33,4 @@ obj-$(CONFIG_EP93XX_DMA) += ep93xx_dma.o
 obj-$(CONFIG_DMA_SA11X0) += sa11x0-dma.o
 obj-$(CONFIG_MMP_TDMA) += mmp_tdma.o
 obj-$(CONFIG_DMA_OMAP) += omap-dma.o
+obj-$(CONFIG_MMP_PDMA) += mmp_pdma.o
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 6fbeebb9486f..d1cc5791476b 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -1892,6 +1892,7 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
         pl08x->pd = dev_get_platdata(&adev->dev);
         if (!pl08x->pd) {
                 dev_err(&adev->dev, "no platform data supplied\n");
+                ret = -EINVAL;
                 goto out_no_platdata;
         }
 
@@ -1943,6 +1944,7 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
                 dev_err(&adev->dev, "%s failed to allocate "
                         "physical channel holders\n",
                         __func__);
+                ret = -ENOMEM;
                 goto out_no_phychans;
         }
 
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index d3c5a5a88f1e..c4b0eb3cde81 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -36,12 +36,22 @@
  * which does not support descriptor writeback.
  */
 
+static inline unsigned int dwc_get_dms(struct dw_dma_slave *slave)
+{
+        return slave ? slave->dst_master : 0;
+}
+
+static inline unsigned int dwc_get_sms(struct dw_dma_slave *slave)
+{
+        return slave ? slave->src_master : 1;
+}
+
 #define DWC_DEFAULT_CTLLO(_chan) ({                             \
                 struct dw_dma_slave *__slave = (_chan->private);        \
                 struct dw_dma_chan *_dwc = to_dw_dma_chan(_chan);       \
                 struct dma_slave_config *_sconfig = &_dwc->dma_sconfig; \
-                int _dms = __slave ? __slave->dst_master : 0;   \
-                int _sms = __slave ? __slave->src_master : 1;   \
+                int _dms = dwc_get_dms(__slave);                \
+                int _sms = dwc_get_sms(__slave);                \
                 u8 _smsize = __slave ? _sconfig->src_maxburst : \
                         DW_DMA_MSIZE_16;                        \
                 u8 _dmsize = __slave ? _sconfig->dst_maxburst : \
@@ -56,16 +66,6 @@
         })
 
 /*
- * This is configuration-dependent and usually a funny size like 4095.
- *
- * Note that this is a transfer count, i.e. if we transfer 32-bit
- * words, we can do 16380 bytes per descriptor.
- *
- * This parameter is also system-specific.
- */
-#define DWC_MAX_COUNT   4095U
-
-/*
  * Number of descriptors to allocate for each channel. This should be
  * made configurable somehow; preferably, the clients (at least the
  * ones using slave transfers) should be able to give us a hint.
@@ -177,6 +177,11 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 
                 cfghi = dws->cfg_hi;
                 cfglo |= dws->cfg_lo & ~DWC_CFGL_CH_PRIOR_MASK;
+        } else {
+                if (dwc->dma_sconfig.direction == DMA_MEM_TO_DEV)
+                        cfghi = DWC_CFGH_DST_PER(dwc->dma_sconfig.slave_id);
+                else if (dwc->dma_sconfig.direction == DMA_DEV_TO_MEM)
+                        cfghi = DWC_CFGH_SRC_PER(dwc->dma_sconfig.slave_id);
         }
 
         channel_writel(dwc, CFG_LO, cfglo);
@@ -206,7 +211,7 @@ static inline unsigned int dwc_fast_fls(unsigned long long v)
         return 0;
 }
 
-static void dwc_dump_chan_regs(struct dw_dma_chan *dwc)
+static inline void dwc_dump_chan_regs(struct dw_dma_chan *dwc)
 {
         dev_err(chan2dev(&dwc->chan),
                 "  SAR: 0x%x DAR: 0x%x LLP: 0x%x CTL: 0x%x:%08x\n",
@@ -227,10 +232,29 @@ static inline void dwc_chan_disable(struct dw_dma *dw, struct dw_dma_chan *dwc)
 
 /*----------------------------------------------------------------------*/
 
+/* Perform single block transfer */
+static inline void dwc_do_single_block(struct dw_dma_chan *dwc,
+                                       struct dw_desc *desc)
+{
+        struct dw_dma   *dw = to_dw_dma(dwc->chan.device);
+        u32             ctllo;
+
+        /* Software emulation of LLP mode relies on interrupts to continue
+         * multi block transfer. */
+        ctllo = desc->lli.ctllo | DWC_CTLL_INT_EN;
+
+        channel_writel(dwc, SAR, desc->lli.sar);
+        channel_writel(dwc, DAR, desc->lli.dar);
+        channel_writel(dwc, CTL_LO, ctllo);
+        channel_writel(dwc, CTL_HI, desc->lli.ctlhi);
+        channel_set_bit(dw, CH_EN, dwc->mask);
+}
+
 /* Called with dwc->lock held and bh disabled */
 static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 {
         struct dw_dma   *dw = to_dw_dma(dwc->chan.device);
+        unsigned long   was_soft_llp;
 
         /* ASSERT:  channel is idle */
         if (dma_readl(dw, CH_EN) & dwc->mask) {
@@ -242,6 +266,26 @@ static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
                 return;
         }
 
+        if (dwc->nollp) {
+                was_soft_llp = test_and_set_bit(DW_DMA_IS_SOFT_LLP,
+                                                &dwc->flags);
+                if (was_soft_llp) {
+                        dev_err(chan2dev(&dwc->chan),
+                                "BUG: Attempted to start new LLP transfer "
+                                "inside ongoing one\n");
+                        return;
+                }
+
+                dwc_initialize(dwc);
+
+                dwc->tx_list = &first->tx_list;
+                dwc->tx_node_active = first->tx_list.next;
+
+                dwc_do_single_block(dwc, first);
+
+                return;
+        }
+
         dwc_initialize(dwc);
 
         channel_writel(dwc, LLP, first->txd.phys);
@@ -553,8 +597,36 @@ static void dw_dma_tasklet(unsigned long data)
                         dwc_handle_cyclic(dw, dwc, status_err, status_xfer);
                 else if (status_err & (1 << i))
                         dwc_handle_error(dw, dwc);
-                else if (status_xfer & (1 << i))
+                else if (status_xfer & (1 << i)) {
+                        unsigned long flags;
+
+                        spin_lock_irqsave(&dwc->lock, flags);
+                        if (test_bit(DW_DMA_IS_SOFT_LLP, &dwc->flags)) {
+                                if (dwc->tx_node_active != dwc->tx_list) {
+                                        struct dw_desc *desc =
+                                                list_entry(dwc->tx_node_active,
+                                                           struct dw_desc,
+                                                           desc_node);
+
+                                        dma_writel(dw, CLEAR.XFER, dwc->mask);
+
+                                        /* move pointer to next descriptor */
+                                        dwc->tx_node_active =
+                                                dwc->tx_node_active->next;
+
+                                        dwc_do_single_block(dwc, desc);
+
+                                        spin_unlock_irqrestore(&dwc->lock, flags);
+                                        continue;
+                                } else {
+                                        /* we are done here */
+                                        clear_bit(DW_DMA_IS_SOFT_LLP, &dwc->flags);
+                                }
+                        }
+                        spin_unlock_irqrestore(&dwc->lock, flags);
+
                         dwc_scan_descriptors(dw, dwc);
+                }
         }
 
         /*
@@ -636,6 +708,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
                 size_t len, unsigned long flags)
 {
         struct dw_dma_chan      *dwc = to_dw_dma_chan(chan);
+        struct dw_dma_slave     *dws = chan->private;
         struct dw_desc          *desc;
         struct dw_desc          *first;
         struct dw_desc          *prev;
@@ -643,6 +716,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
         size_t                  offset;
         unsigned int            src_width;
         unsigned int            dst_width;
+        unsigned int            data_width;
         u32                     ctllo;
 
         dev_vdbg(chan2dev(chan),
@@ -655,7 +729,11 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
                 return NULL;
         }
 
-        src_width = dst_width = dwc_fast_fls(src | dest | len);
+        data_width = min_t(unsigned int, dwc->dw->data_width[dwc_get_sms(dws)],
+                                         dwc->dw->data_width[dwc_get_dms(dws)]);
+
+        src_width = dst_width = min_t(unsigned int, data_width,
+                                      dwc_fast_fls(src | dest | len));
 
         ctllo = DWC_DEFAULT_CTLLO(chan)
                         | DWC_CTLL_DST_WIDTH(dst_width)
@@ -667,7 +745,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 
         for (offset = 0; offset < len; offset += xfer_count << src_width) {
                 xfer_count = min_t(size_t, (len - offset) >> src_width,
-                                DWC_MAX_COUNT);
+                                           dwc->block_size);
 
                 desc = dwc_desc_get(dwc);
                 if (!desc)
@@ -725,6 +803,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
         dma_addr_t              reg;
         unsigned int            reg_width;
         unsigned int            mem_width;
+        unsigned int            data_width;
         unsigned int            i;
         struct scatterlist      *sg;
         size_t                  total_len = 0;
@@ -748,6 +827,8 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
                 ctllo |= sconfig->device_fc ? DWC_CTLL_FC(DW_DMA_FC_P_M2P) :
                         DWC_CTLL_FC(DW_DMA_FC_D_M2P);
 
+                data_width = dwc->dw->data_width[dwc_get_sms(dws)];
+
                 for_each_sg(sgl, sg, sg_len, i) {
                         struct dw_desc  *desc;
                         u32             len, dlen, mem;
@@ -755,7 +836,8 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
                         mem = sg_dma_address(sg);
                         len = sg_dma_len(sg);
 
-                        mem_width = dwc_fast_fls(mem | len);
+                        mem_width = min_t(unsigned int,
+                                          data_width, dwc_fast_fls(mem | len));
 
 slave_sg_todev_fill_desc:
                         desc = dwc_desc_get(dwc);
@@ -768,8 +850,8 @@ slave_sg_todev_fill_desc:
                         desc->lli.sar = mem;
                         desc->lli.dar = reg;
                         desc->lli.ctllo = ctllo | DWC_CTLL_SRC_WIDTH(mem_width);
-                        if ((len >> mem_width) > DWC_MAX_COUNT) {
-                                dlen = DWC_MAX_COUNT << mem_width;
+                        if ((len >> mem_width) > dwc->block_size) {
+                                dlen = dwc->block_size << mem_width;
                                 mem += dlen;
                                 len -= dlen;
                         } else {
@@ -808,6 +890,8 @@ slave_sg_todev_fill_desc:
                 ctllo |= sconfig->device_fc ? DWC_CTLL_FC(DW_DMA_FC_P_P2M) :
                         DWC_CTLL_FC(DW_DMA_FC_D_P2M);
 
+                data_width = dwc->dw->data_width[dwc_get_dms(dws)];
+
                 for_each_sg(sgl, sg, sg_len, i) {
                         struct dw_desc  *desc;
                         u32             len, dlen, mem;
@@ -815,7 +899,8 @@ slave_sg_todev_fill_desc:
                         mem = sg_dma_address(sg);
                         len = sg_dma_len(sg);
 
-                        mem_width = dwc_fast_fls(mem | len);
+                        mem_width = min_t(unsigned int,
+                                          data_width, dwc_fast_fls(mem | len));
 
 slave_sg_fromdev_fill_desc:
                         desc = dwc_desc_get(dwc);
@@ -828,8 +913,8 @@ slave_sg_fromdev_fill_desc:
                         desc->lli.sar = reg;
                         desc->lli.dar = mem;
                         desc->lli.ctllo = ctllo | DWC_CTLL_DST_WIDTH(mem_width);
-                        if ((len >> reg_width) > DWC_MAX_COUNT) {
-                                dlen = DWC_MAX_COUNT << reg_width;
+                        if ((len >> reg_width) > dwc->block_size) {
+                                dlen = dwc->block_size << reg_width;
                                 mem += dlen;
                                 len -= dlen;
                         } else {
@@ -945,6 +1030,8 @@ static int dwc_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
         } else if (cmd == DMA_TERMINATE_ALL) {
                 spin_lock_irqsave(&dwc->lock, flags);
 
+                clear_bit(DW_DMA_IS_SOFT_LLP, &dwc->flags);
+
                 dwc_chan_disable(dw, dwc);
 
                 dwc->paused = false;
@@ -1187,6 +1274,13 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
         unsigned long                   flags;
 
         spin_lock_irqsave(&dwc->lock, flags);
+        if (dwc->nollp) {
+                spin_unlock_irqrestore(&dwc->lock, flags);
+                dev_dbg(chan2dev(&dwc->chan),
+                                "channel doesn't support LLP transfers\n");
+                return ERR_PTR(-EINVAL);
+        }
+
         if (!list_empty(&dwc->queue) || !list_empty(&dwc->active_list)) {
                 spin_unlock_irqrestore(&dwc->lock, flags);
                 dev_dbg(chan2dev(&dwc->chan),
@@ -1212,7 +1306,7 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
         periods = buf_len / period_len;
 
         /* Check for too big/unaligned periods and unaligned DMA buffer. */
-        if (period_len > (DWC_MAX_COUNT << reg_width))
+        if (period_len > (dwc->block_size << reg_width))
                 goto out_err;
         if (unlikely(period_len & ((1 << reg_width) - 1)))
                 goto out_err;
@@ -1374,6 +1468,11 @@ static int __devinit dw_probe(struct platform_device *pdev)
         struct resource         *io;
         struct dw_dma           *dw;
         size_t                  size;
+        void __iomem            *regs;
+        bool                    autocfg;
+        unsigned int            dw_params;
+        unsigned int            nr_channels;
+        unsigned int            max_blk_size = 0;
         int                     irq;
         int                     err;
         int                     i;
@@ -1390,32 +1489,46 @@ static int __devinit dw_probe(struct platform_device *pdev)
         if (irq < 0)
                 return irq;
 
-        size = sizeof(struct dw_dma);
-        size += pdata->nr_channels * sizeof(struct dw_dma_chan);
-        dw = kzalloc(size, GFP_KERNEL);
+        regs = devm_request_and_ioremap(&pdev->dev, io);
+        if (!regs)
+                return -EBUSY;
+
+        dw_params = dma_read_byaddr(regs, DW_PARAMS);
+        autocfg = dw_params >> DW_PARAMS_EN & 0x1;
+
+        if (autocfg)
+                nr_channels = (dw_params >> DW_PARAMS_NR_CHAN & 0x7) + 1;
+        else
+                nr_channels = pdata->nr_channels;
+
+        size = sizeof(struct dw_dma) + nr_channels * sizeof(struct dw_dma_chan);
+        dw = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
         if (!dw)
                 return -ENOMEM;
 
-        if (!request_mem_region(io->start, DW_REGLEN, pdev->dev.driver->name)) {
-                err = -EBUSY;
-                goto err_kfree;
-        }
+        dw->clk = devm_clk_get(&pdev->dev, "hclk");
+        if (IS_ERR(dw->clk))
+                return PTR_ERR(dw->clk);
+        clk_prepare_enable(dw->clk);
 
-        dw->regs = ioremap(io->start, DW_REGLEN);
-        if (!dw->regs) {
-                err = -ENOMEM;
-                goto err_release_r;
-        }
+        dw->regs = regs;
+
+        /* get hardware configuration parameters */
+        if (autocfg) {
+                max_blk_size = dma_readl(dw, MAX_BLK_SIZE);
 
-        dw->clk = clk_get(&pdev->dev, "hclk");
-        if (IS_ERR(dw->clk)) {
-                err = PTR_ERR(dw->clk);
-                goto err_clk;
+                dw->nr_masters = (dw_params >> DW_PARAMS_NR_MASTER & 3) + 1;
+                for (i = 0; i < dw->nr_masters; i++) {
+                        dw->data_width[i] =
+                                (dw_params >> DW_PARAMS_DATA_WIDTH(i) & 3) + 2;
+                }
+        } else {
+                dw->nr_masters = pdata->nr_masters;
+                memcpy(dw->data_width, pdata->data_width, 4);
         }
-        clk_prepare_enable(dw->clk);
 
         /* Calculate all channel mask before DMA setup */
-        dw->all_chan_mask = (1 << pdata->nr_channels) - 1;
+        dw->all_chan_mask = (1 << nr_channels) - 1;
 
         /* force dma off, just in case */
         dw_dma_off(dw);
@@ -1423,17 +1536,19 @@ static int __devinit dw_probe(struct platform_device *pdev)
         /* disable BLOCK interrupts as well */
         channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 
-        err = request_irq(irq, dw_dma_interrupt, 0, "dw_dmac", dw);
+        err = devm_request_irq(&pdev->dev, irq, dw_dma_interrupt, 0,
+                               "dw_dmac", dw);
         if (err)
-                goto err_irq;
+                return err;
 
         platform_set_drvdata(pdev, dw);
 
         tasklet_init(&dw->tasklet, dw_dma_tasklet, (unsigned long)dw);
 
         INIT_LIST_HEAD(&dw->dma.channels);
-        for (i = 0; i < pdata->nr_channels; i++) {
+        for (i = 0; i < nr_channels; i++) {
                 struct dw_dma_chan      *dwc = &dw->chan[i];
+                int                     r = nr_channels - i - 1;
 
                 dwc->chan.device = &dw->dma;
                 dma_cookie_init(&dwc->chan);
@@ -1445,7 +1560,7 @@ static int __devinit dw_probe(struct platform_device *pdev)
 
                 /* 7 is highest priority & 0 is lowest. */
                 if (pdata->chan_priority == CHAN_PRIORITY_ASCENDING)
-                        dwc->priority = pdata->nr_channels - i - 1;
+                        dwc->priority = r;
                 else
                         dwc->priority = i;
 
@@ -1458,6 +1573,32 @@ static int __devinit dw_probe(struct platform_device *pdev)
                 INIT_LIST_HEAD(&dwc->free_list);
 
                 channel_clear_bit(dw, CH_EN, dwc->mask);
+
+                dwc->dw = dw;
+
+                /* hardware configuration */
+                if (autocfg) {
+                        unsigned int dwc_params;
+
+                        dwc_params = dma_read_byaddr(regs + r * sizeof(u32),
+                                                     DWC_PARAMS);
+
+                        /* Decode maximum block size for given channel. The
+                         * stored 4 bit value represents blocks from 0x00 for 3
+                         * up to 0x0a for 4095. */
+                        dwc->block_size =
+                                (4 << ((max_blk_size >> 4 * i) & 0xf)) - 1;
+                        dwc->nollp =
+                                (dwc_params >> DWC_PARAMS_MBLK_EN & 0x1) == 0;
+                } else {
+                        dwc->block_size = pdata->block_size;
+
+                        /* Check if channel supports multi block transfer */
+                        channel_writel(dwc, LLP, 0xfffffffc);
+                        dwc->nollp =
+                                (channel_readl(dwc, LLP) & 0xfffffffc) == 0;
+                        channel_writel(dwc, LLP, 0);
+                }
         }
 
         /* Clear all interrupts on all channels. */
@@ -1486,35 +1627,21 @@ static int __devinit dw_probe(struct platform_device *pdev)
         dma_writel(dw, CFG, DW_CFG_DMA_EN);
 
         printk(KERN_INFO "%s: DesignWare DMA Controller, %d channels\n",
-                        dev_name(&pdev->dev), pdata->nr_channels);
+                        dev_name(&pdev->dev), nr_channels);
 
         dma_async_device_register(&dw->dma);
 
         return 0;
-
-err_irq:
-        clk_disable_unprepare(dw->clk);
-        clk_put(dw->clk);
-err_clk:
-        iounmap(dw->regs);
-        dw->regs = NULL;
-err_release_r:
-        release_resource(io);
-err_kfree:
-        kfree(dw);
-        return err;
 }
 
 static int __devexit dw_remove(struct platform_device *pdev)
 {
         struct dw_dma           *dw = platform_get_drvdata(pdev);
         struct dw_dma_chan      *dwc, *_dwc;
-        struct resource         *io;
 
         dw_dma_off(dw);
         dma_async_device_unregister(&dw->dma);
 
-        free_irq(platform_get_irq(pdev, 0), dw);
         tasklet_kill(&dw->tasklet);
 
         list_for_each_entry_safe(dwc, _dwc, &dw->dma.channels,
@@ -1523,17 +1650,6 @@ static int __devexit dw_remove(struct platform_device *pdev)
                 channel_clear_bit(dw, CH_EN, dwc->mask);
         }
 
-        clk_disable_unprepare(dw->clk);
-        clk_put(dw->clk);
-
-        iounmap(dw->regs);
-        dw->regs = NULL;
-
-        io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        release_mem_region(io->start, DW_REGLEN);
-
-        kfree(dw);
-
         return 0;
 }
 
diff --git a/drivers/dma/dw_dmac_regs.h b/drivers/dma/dw_dmac_regs.h
index 50830bee087a..88965597b7d0 100644
--- a/drivers/dma/dw_dmac_regs.h
+++ b/drivers/dma/dw_dmac_regs.h
@@ -82,9 +82,47 @@ struct dw_dma_regs {
         DW_REG(ID);
         DW_REG(TEST);
 
+        /* reserved */
+        DW_REG(__reserved0);
+        DW_REG(__reserved1);
+
         /* optional encoded params, 0x3c8..0x3f7 */
+        u32     __reserved;
+
+        /* per-channel configuration registers */
+        u32     DWC_PARAMS[DW_DMA_MAX_NR_CHANNELS];
+        u32     MULTI_BLK_TYPE;
+        u32     MAX_BLK_SIZE;
+
+        /* top-level parameters */
+        u32     DW_PARAMS;
 };
 
+#ifdef CONFIG_DW_DMAC_BIG_ENDIAN_IO
+#define dma_readl_native ioread32be
+#define dma_writel_native iowrite32be
+#else
+#define dma_readl_native readl
+#define dma_writel_native writel
+#endif
+
+/* To access the registers in early stage of probe */
+#define dma_read_byaddr(addr, name) \
+        dma_readl_native((addr) + offsetof(struct dw_dma_regs, name))
+
+/* Bitfields in DW_PARAMS */
+#define DW_PARAMS_NR_CHAN       8               /* number of channels */
+#define DW_PARAMS_NR_MASTER     11              /* number of AHB masters */
+#define DW_PARAMS_DATA_WIDTH(n) (15 + 2 * (n))
+#define DW_PARAMS_DATA_WIDTH1   15              /* master 1 data width */
+#define DW_PARAMS_DATA_WIDTH2   17              /* master 2 data width */
+#define DW_PARAMS_DATA_WIDTH3   19              /* master 3 data width */
+#define DW_PARAMS_DATA_WIDTH4   21              /* master 4 data width */
+#define DW_PARAMS_EN            28              /* encoded parameters */
+
+/* Bitfields in DWC_PARAMS */
+#define DWC_PARAMS_MBLK_EN      11              /* multi block transfer */
+
 /* Bitfields in CTL_LO */
 #define DWC_CTLL_INT_EN         (1 << 0)        /* irqs enabled? */
 #define DWC_CTLL_DST_WIDTH(n)   ((n)<<1)        /* bytes per element */
@@ -140,10 +178,9 @@ struct dw_dma_regs {
 /* Bitfields in CFG */
 #define DW_CFG_DMA_EN           (1 << 0)
 
-#define DW_REGLEN               0x400
-
 enum dw_dmac_flags {
         DW_DMA_IS_CYCLIC = 0,
+        DW_DMA_IS_SOFT_LLP = 1,
 };
 
 struct dw_dma_chan {
@@ -154,6 +191,10 @@ struct dw_dma_chan {
         bool                    paused;
         bool                    initialized;
 
+        /* software emulation of the LLP transfers */
+        struct list_head        *tx_list;
+        struct list_head        *tx_node_active;
+
         spinlock_t              lock;
 
         /* these other elements are all protected by lock */
@@ -165,8 +206,15 @@ struct dw_dma_chan {
 
         unsigned int            descs_allocated;
 
+        /* hardware configuration */
+        unsigned int            block_size;
+        bool                    nollp;
+
         /* configuration passed via DMA_SLAVE_CONFIG */
         struct dma_slave_config dma_sconfig;
+
+        /* backlink to dw_dma */
+        struct dw_dma           *dw;
 };
 
 static inline struct dw_dma_chan_regs __iomem *
@@ -176,9 +224,9 @@ __dwc_regs(struct dw_dma_chan *dwc)
 }
 
 #define channel_readl(dwc, name) \
-        readl(&(__dwc_regs(dwc)->name))
+        dma_readl_native(&(__dwc_regs(dwc)->name))
 #define channel_writel(dwc, name, val) \
-        writel((val), &(__dwc_regs(dwc)->name))
+        dma_writel_native((val), &(__dwc_regs(dwc)->name))
 
 static inline struct dw_dma_chan *to_dw_dma_chan(struct dma_chan *chan)
 {
@@ -193,6 +241,10 @@ struct dw_dma {
 
         u8                      all_chan_mask;
 
+        /* hardware configuration */
+        unsigned char           nr_masters;
+        unsigned char           data_width[4];
+
         struct dw_dma_chan      chan[0];
 };
 
@@ -202,9 +254,9 @@ static inline struct dw_dma_regs __iomem *__dw_regs(struct dw_dma *dw)
 }
 
 #define dma_readl(dw, name) \
-        readl(&(__dw_regs(dw)->name))
+        dma_readl_native(&(__dw_regs(dw)->name))
 #define dma_writel(dw, name, val) \
-        writel((val), &(__dw_regs(dw)->name))
+        dma_writel_native((val), &(__dw_regs(dw)->name))
 
 #define channel_set_bit(dw, reg, mask) \
         dma_writel(dw, reg, ((mask) << 8) | (mask))
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
new file mode 100644
index 000000000000..05aea3ce8506
--- /dev/null
+++ b/drivers/dma/edma.c
@@ -0,0 +1,671 @@
+/*
+ * TI EDMA DMA engine driver
+ *
+ * Copyright 2012 Texas Instruments
+ *
+ * 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 version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include <mach/edma.h>
+
+#include "dmaengine.h"
+#include "virt-dma.h"
+
+/*
+ * This will go away when the private EDMA API is folded
+ * into this driver and the platform device(s) are
+ * instantiated in the arch code. We can only get away
+ * with this simplification because DA8XX may not be built
+ * in the same kernel image with other DaVinci parts. This
+ * avoids having to sprinkle dmaengine driver platform devices
+ * and data throughout all the existing board files.
+ */
+#ifdef CONFIG_ARCH_DAVINCI_DA8XX
+#define EDMA_CTLRS      2
+#define EDMA_CHANS      32
+#else
+#define EDMA_CTLRS      1
+#define EDMA_CHANS      64
+#endif /* CONFIG_ARCH_DAVINCI_DA8XX */
+
+/* Max of 16 segments per channel to conserve PaRAM slots */
+#define MAX_NR_SG               16
+#define EDMA_MAX_SLOTS          MAX_NR_SG
+#define EDMA_DESCRIPTORS        16
+
+struct edma_desc {
+        struct virt_dma_desc            vdesc;
+        struct list_head                node;
+        int                             absync;
+        int                             pset_nr;
+        struct edmacc_param             pset[0];
+};
+
+struct edma_cc;
+
+struct edma_chan {
+        struct virt_dma_chan            vchan;
+        struct list_head                node;
+        struct edma_desc                *edesc;
+        struct edma_cc                  *ecc;
+        int                             ch_num;
+        bool                            alloced;
+        int                             slot[EDMA_MAX_SLOTS];
+        dma_addr_t                      addr;
+        int                             addr_width;
+        int                             maxburst;
+};
+
+struct edma_cc {
+        int                             ctlr;
+        struct dma_device               dma_slave;
+        struct edma_chan                slave_chans[EDMA_CHANS];
+        int                             num_slave_chans;
+        int                             dummy_slot;
+};
+
+static inline struct edma_cc *to_edma_cc(struct dma_device *d)
+{
+        return container_of(d, struct edma_cc, dma_slave);
+}
+
+static inline struct edma_chan *to_edma_chan(struct dma_chan *c)
+{
+        return container_of(c, struct edma_chan, vchan.chan);
+}
+
+static inline struct edma_desc
+*to_edma_desc(struct dma_async_tx_descriptor *tx)
+{
+        return container_of(tx, struct edma_desc, vdesc.tx);
+}
+
+static void edma_desc_free(struct virt_dma_desc *vdesc)
+{
+        kfree(container_of(vdesc, struct edma_desc, vdesc));
+}
+
+/* Dispatch a queued descriptor to the controller (caller holds lock) */
+static void edma_execute(struct edma_chan *echan)
+{
+        struct virt_dma_desc *vdesc = vchan_next_desc(&echan->vchan);
+        struct edma_desc *edesc;
+        int i;
+
+        if (!vdesc) {
+                echan->edesc = NULL;
+                return;
+        }
+
+        list_del(&vdesc->node);
+
+        echan->edesc = edesc = to_edma_desc(&vdesc->tx);
+
+        /* Write descriptor PaRAM set(s) */
+        for (i = 0; i < edesc->pset_nr; i++) {
+                edma_write_slot(echan->slot[i], &edesc->pset[i]);
+                dev_dbg(echan->vchan.chan.device->dev,
+                        "\n pset[%d]:\n"
+                        "  chnum\t%d\n"
+                        "  slot\t%d\n"
+                        "  opt\t%08x\n"
+                        "  src\t%08x\n"
+                        "  dst\t%08x\n"
+                        "  abcnt\t%08x\n"
+                        "  ccnt\t%08x\n"
+                        "  bidx\t%08x\n"
+                        "  cidx\t%08x\n"
+                        "  lkrld\t%08x\n",
+                        i, echan->ch_num, echan->slot[i],
+                        edesc->pset[i].opt,
+                        edesc->pset[i].src,
+                        edesc->pset[i].dst,
+                        edesc->pset[i].a_b_cnt,
+                        edesc->pset[i].ccnt,
+                        edesc->pset[i].src_dst_bidx,
+                        edesc->pset[i].src_dst_cidx,
+                        edesc->pset[i].link_bcntrld);
+                /* Link to the previous slot if not the last set */
+                if (i != (edesc->pset_nr - 1))
+                        edma_link(echan->slot[i], echan->slot[i+1]);
+                /* Final pset links to the dummy pset */
+                else
+                        edma_link(echan->slot[i], echan->ecc->dummy_slot);
+        }
+
+        edma_start(echan->ch_num);
+}
+
+static int edma_terminate_all(struct edma_chan *echan)
+{
+        unsigned long flags;
+        LIST_HEAD(head);
+
+        spin_lock_irqsave(&echan->vchan.lock, flags);
+
+        /*
+         * Stop DMA activity: we assume the callback will not be called
+         * after edma_dma() returns (even if it does, it will see
+         * echan->edesc is NULL and exit.)
+         */
+        if (echan->edesc) {
+                echan->edesc = NULL;
+                edma_stop(echan->ch_num);
+        }
+
+        vchan_get_all_descriptors(&echan->vchan, &head);
+        spin_unlock_irqrestore(&echan->vchan.lock, flags);
+        vchan_dma_desc_free_list(&echan->vchan, &head);
+
+        return 0;
+}
+
+
+static int edma_slave_config(struct edma_chan *echan,
+        struct dma_slave_config *config)
+{
+        if ((config->src_addr_width > DMA_SLAVE_BUSWIDTH_4_BYTES) ||
+            (config->dst_addr_width > DMA_SLAVE_BUSWIDTH_4_BYTES))
+                return -EINVAL;
+
+        if (config->direction == DMA_MEM_TO_DEV) {
+                if (config->dst_addr)
+                        echan->addr = config->dst_addr;
+                if (config->dst_addr_width)
+                        echan->addr_width = config->dst_addr_width;
+                if (config->dst_maxburst)
+                        echan->maxburst = config->dst_maxburst;
+        } else if (config->direction == DMA_DEV_TO_MEM) {
+                if (config->src_addr)
+                        echan->addr = config->src_addr;
+                if (config->src_addr_width)
+                        echan->addr_width = config->src_addr_width;
+                if (config->src_maxburst)
+                        echan->maxburst = config->src_maxburst;
+        }
+
+        return 0;
+}
+
+static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+                        unsigned long arg)
+{
+        int ret = 0;
+        struct dma_slave_config *config;
+        struct edma_chan *echan = to_edma_chan(chan);
+
+        switch (cmd) {
+        case DMA_TERMINATE_ALL:
+                edma_terminate_all(echan);
+                break;
+        case DMA_SLAVE_CONFIG:
+                config = (struct dma_slave_config *)arg;
+                ret = edma_slave_config(echan, config);
+                break;
+        default:
+                ret = -ENOSYS;
+        }
+
+        return ret;
+}
+
+static struct dma_async_tx_descriptor *edma_prep_slave_sg(
+        struct dma_chan *chan, struct scatterlist *sgl,
+        unsigned int sg_len, enum dma_transfer_direction direction,
+        unsigned long tx_flags, void *context)
+{
+        struct edma_chan *echan = to_edma_chan(chan);
+        struct device *dev = chan->device->dev;
+        struct edma_desc *edesc;
+        struct scatterlist *sg;
+        int i;
+        int acnt, bcnt, ccnt, src, dst, cidx;
+        int src_bidx, dst_bidx, src_cidx, dst_cidx;
+
+        if (unlikely(!echan || !sgl || !sg_len))
+                return NULL;
+
+        if (echan->addr_width == DMA_SLAVE_BUSWIDTH_UNDEFINED) {
+                dev_err(dev, "Undefined slave buswidth\n");
+                return NULL;
+        }
+
+        if (sg_len > MAX_NR_SG) {
+                dev_err(dev, "Exceeded max SG segments %d > %d\n",
+                        sg_len, MAX_NR_SG);
+                return NULL;
+        }
+
+        edesc = kzalloc(sizeof(*edesc) + sg_len *
+                sizeof(edesc->pset[0]), GFP_ATOMIC);
+        if (!edesc) {
+                dev_dbg(dev, "Failed to allocate a descriptor\n");
+                return NULL;
+        }
+
+        edesc->pset_nr = sg_len;
+
+        for_each_sg(sgl, sg, sg_len, i) {
+                /* Allocate a PaRAM slot, if needed */
+                if (echan->slot[i] < 0) {
+                        echan->slot[i] =
+                                edma_alloc_slot(EDMA_CTLR(echan->ch_num),
+                                                EDMA_SLOT_ANY);
+                        if (echan->slot[i] < 0) {
+                                dev_err(dev, "Failed to allocate slot\n");
+                                return NULL;
+                        }
+                }
+
+                acnt = echan->addr_width;
+
+                /*
+                 * If the maxburst is equal to the fifo width, use
+                 * A-synced transfers. This allows for large contiguous
+                 * buffer transfers using only one PaRAM set.
+                 */
+                if (echan->maxburst == 1) {
+                        edesc->absync = false;
+                        ccnt = sg_dma_len(sg) / acnt / (SZ_64K - 1);
+                        bcnt = sg_dma_len(sg) / acnt - ccnt * (SZ_64K - 1);
+                        if (bcnt)
+                                ccnt++;
+                        else
+                                bcnt = SZ_64K - 1;
+                        cidx = acnt;
+                /*
+                 * If maxburst is greater than the fifo address_width,
+                 * use AB-synced transfers where A count is the fifo
+                 * address_width and B count is the maxburst. In this
+                 * case, we are limited to transfers of C count frames
+                 * of (address_width * maxburst) where C count is limited
+                 * to SZ_64K-1. This places an upper bound on the length
+                 * of an SG segment that can be handled.
+                 */
+                } else {
+                        edesc->absync = true;
+                        bcnt = echan->maxburst;
+                        ccnt = sg_dma_len(sg) / (acnt * bcnt);
+                        if (ccnt > (SZ_64K - 1)) {
+                                dev_err(dev, "Exceeded max SG segment size\n");
+                                return NULL;
+                        }
+                        cidx = acnt * bcnt;
+                }
+
+                if (direction == DMA_MEM_TO_DEV) {
+                        src = sg_dma_address(sg);
+                        dst = echan->addr;
+                        src_bidx = acnt;
+                        src_cidx = cidx;
+                        dst_bidx = 0;
+                        dst_cidx = 0;
+                } else {
+                        src = echan->addr;
+                        dst = sg_dma_address(sg);
+                        src_bidx = 0;
+                        src_cidx = 0;
+                        dst_bidx = acnt;
+                        dst_cidx = cidx;
+                }
+
+                edesc->pset[i].opt = EDMA_TCC(EDMA_CHAN_SLOT(echan->ch_num));
+                /* Configure A or AB synchronized transfers */
+                if (edesc->absync)
+                        edesc->pset[i].opt |= SYNCDIM;
+                /* If this is the last set, enable completion interrupt flag */
+                if (i == sg_len - 1)
+                        edesc->pset[i].opt |= TCINTEN;
+
+                edesc->pset[i].src = src;
+                edesc->pset[i].dst = dst;
+
+                edesc->pset[i].src_dst_bidx = (dst_bidx << 16) | src_bidx;
+                edesc->pset[i].src_dst_cidx = (dst_cidx << 16) | src_cidx;
+
+                edesc->pset[i].a_b_cnt = bcnt << 16 | acnt;
+                edesc->pset[i].ccnt = ccnt;
+                edesc->pset[i].link_bcntrld = 0xffffffff;
+
+        }
+
+        return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
+}
+
+static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
+{
+        struct edma_chan *echan = data;
+        struct device *dev = echan->vchan.chan.device->dev;
+        struct edma_desc *edesc;
+        unsigned long flags;
+
+        /* Stop the channel */
+        edma_stop(echan->ch_num);
+
+        switch (ch_status) {
+        case DMA_COMPLETE:
+                dev_dbg(dev, "transfer complete on channel %d\n", ch_num);
+
+                spin_lock_irqsave(&echan->vchan.lock, flags);
+
+                edesc = echan->edesc;
+                if (edesc) {
+                        edma_execute(echan);
+                        vchan_cookie_complete(&edesc->vdesc);
+                }
+
+                spin_unlock_irqrestore(&echan->vchan.lock, flags);
+
+                break;
+        case DMA_CC_ERROR:
+                dev_dbg(dev, "transfer error on channel %d\n", ch_num);
+                break;
+        default:
+                break;
+        }
+}
+
+/* Alloc channel resources */
+static int edma_alloc_chan_resources(struct dma_chan *chan)
+{
+        struct edma_chan *echan = to_edma_chan(chan);
+        struct device *dev = chan->device->dev;
+        int ret;
+        int a_ch_num;
+        LIST_HEAD(descs);
+
+        a_ch_num = edma_alloc_channel(echan->ch_num, edma_callback,
+                                        chan, EVENTQ_DEFAULT);
+
+        if (a_ch_num < 0) {
+                ret = -ENODEV;
+                goto err_no_chan;
+        }
+
+        if (a_ch_num != echan->ch_num) {
+                dev_err(dev, "failed to allocate requested channel %u:%u\n",
+                        EDMA_CTLR(echan->ch_num),
+                        EDMA_CHAN_SLOT(echan->ch_num));
+                ret = -ENODEV;
+                goto err_wrong_chan;
+        }
+
+        echan->alloced = true;
+        echan->slot[0] = echan->ch_num;
+
+        dev_info(dev, "allocated channel for %u:%u\n",
+                 EDMA_CTLR(echan->ch_num), EDMA_CHAN_SLOT(echan->ch_num));
+
+        return 0;
+
+err_wrong_chan:
+        edma_free_channel(a_ch_num);
+err_no_chan:
+        return ret;
+}
+
+/* Free channel resources */
+static void edma_free_chan_resources(struct dma_chan *chan)
+{
+        struct edma_chan *echan = to_edma_chan(chan);
+        struct device *dev = chan->device->dev;
+        int i;
+
+        /* Terminate transfers */
+        edma_stop(echan->ch_num);
+
+        vchan_free_chan_resources(&echan->vchan);
+
+        /* Free EDMA PaRAM slots */
+        for (i = 1; i < EDMA_MAX_SLOTS; i++) {
+                if (echan->slot[i] >= 0) {
+                        edma_free_slot(echan->slot[i]);
+                        echan->slot[i] = -1;
+                }
+        }
+
+        /* Free EDMA channel */
+        if (echan->alloced) {
+                edma_free_channel(echan->ch_num);
+                echan->alloced = false;
+        }
+
+        dev_info(dev, "freeing channel for %u\n", echan->ch_num);
+}
+
+/* Send pending descriptor to hardware */
+static void edma_issue_pending(struct dma_chan *chan)
+{
+        struct edma_chan *echan = to_edma_chan(chan);
+        unsigned long flags;
+
+        spin_lock_irqsave(&echan->vchan.lock, flags);
+        if (vchan_issue_pending(&echan->vchan) && !echan->edesc)
+                edma_execute(echan);
+        spin_unlock_irqrestore(&echan->vchan.lock, flags);
+}
+
+static size_t edma_desc_size(struct edma_desc *edesc)
+{
+        int i;
+        size_t size;
+
+        if (edesc->absync)
+                for (size = i = 0; i < edesc->pset_nr; i++)
+                        size += (edesc->pset[i].a_b_cnt & 0xffff) *
+                                (edesc->pset[i].a_b_cnt >> 16) *
+                                 edesc->pset[i].ccnt;
+        else
+                size = (edesc->pset[0].a_b_cnt & 0xffff) *
+                        (edesc->pset[0].a_b_cnt >> 16) +
+                        (edesc->pset[0].a_b_cnt & 0xffff) *
+                        (SZ_64K - 1) * edesc->pset[0].ccnt;
+
+        return size;
+}
+
+/* Check request completion status */
+static enum dma_status edma_tx_status(struct dma_chan *chan,
+                                      dma_cookie_t cookie,
+                                      struct dma_tx_state *txstate)
+{
+        struct edma_chan *echan = to_edma_chan(chan);
+        struct virt_dma_desc *vdesc;
+        enum dma_status ret;
+        unsigned long flags;
+
+        ret = dma_cookie_status(chan, cookie, txstate);
+        if (ret == DMA_SUCCESS || !txstate)
+                return ret;
+
+        spin_lock_irqsave(&echan->vchan.lock, flags);
+        vdesc = vchan_find_desc(&echan->vchan, cookie);
+        if (vdesc) {
+                txstate->residue = edma_desc_size(to_edma_desc(&vdesc->tx));
+        } else if (echan->edesc && echan->edesc->vdesc.tx.cookie == cookie) {
+                struct edma_desc *edesc = echan->edesc;
+                txstate->residue = edma_desc_size(edesc);
+        } else {
+                txstate->residue = 0;
+        }
+        spin_unlock_irqrestore(&echan->vchan.lock, flags);
+
+        return ret;
+}
+
+static void __init edma_chan_init(struct edma_cc *ecc,
+                                  struct dma_device *dma,
+                                  struct edma_chan *echans)
+{
+        int i, j;
+
+        for (i = 0; i < EDMA_CHANS; i++) {
+                struct edma_chan *echan = &echans[i];
+                echan->ch_num = EDMA_CTLR_CHAN(ecc->ctlr, i);
+                echan->ecc = ecc;
+                echan->vchan.desc_free = edma_desc_free;
+
+                vchan_init(&echan->vchan, dma);
+
+                INIT_LIST_HEAD(&echan->node);
+                for (j = 0; j < EDMA_MAX_SLOTS; j++)
+                        echan->slot[j] = -1;
+        }
+}
+
+static void edma_dma_init(struct edma_cc *ecc, struct dma_device *dma,
+                          struct device *dev)
+{
+        dma->device_prep_slave_sg = edma_prep_slave_sg;
+        dma->device_alloc_chan_resources = edma_alloc_chan_resources;
+        dma->device_free_chan_resources = edma_free_chan_resources;
+        dma->device_issue_pending = edma_issue_pending;
+        dma->device_tx_status = edma_tx_status;
+        dma->device_control = edma_control;
+        dma->dev = dev;
+
+        INIT_LIST_HEAD(&dma->channels);
+}
+
+static int __devinit edma_probe(struct platform_device *pdev)
+{
+        struct edma_cc *ecc;
+        int ret;
+
+        ecc = devm_kzalloc(&pdev->dev, sizeof(*ecc), GFP_KERNEL);
+        if (!ecc) {
+                dev_err(&pdev->dev, "Can't allocate controller\n");
+                return -ENOMEM;
+        }
+
+        ecc->ctlr = pdev->id;
+        ecc->dummy_slot = edma_alloc_slot(ecc->ctlr, EDMA_SLOT_ANY);
+        if (ecc->dummy_slot < 0) {
+                dev_err(&pdev->dev, "Can't allocate PaRAM dummy slot\n");
+                return -EIO;
+        }
+
+        dma_cap_zero(ecc->dma_slave.cap_mask);
+        dma_cap_set(DMA_SLAVE, ecc->dma_slave.cap_mask);
+
+        edma_dma_init(ecc, &ecc->dma_slave, &pdev->dev);
+
+        edma_chan_init(ecc, &ecc->dma_slave, ecc->slave_chans);
+
+        ret = dma_async_device_register(&ecc->dma_slave);
+        if (ret)
+                goto err_reg1;
+
+        platform_set_drvdata(pdev, ecc);
+
+        dev_info(&pdev->dev, "TI EDMA DMA engine driver\n");
+
+        return 0;
+
+err_reg1:
+        edma_free_slot(ecc->dummy_slot);
+        return ret;
+}
+
+static int __devexit edma_remove(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct edma_cc *ecc = dev_get_drvdata(dev);
+
+        dma_async_device_unregister(&ecc->dma_slave);
+        edma_free_slot(ecc->dummy_slot);
+
+        return 0;
+}
+
+static struct platform_driver edma_driver = {
+        .probe          = edma_probe,
+        .remove         = __devexit_p(edma_remove),
+        .driver = {
+                .name = "edma-dma-engine",
+                .owner = THIS_MODULE,
+        },
+};
+
+bool edma_filter_fn(struct dma_chan *chan, void *param)
+{
+        if (chan->device->dev->driver == &edma_driver.driver) {
+                struct edma_chan *echan = to_edma_chan(chan);
+                unsigned ch_req = *(unsigned *)param;
+                return ch_req == echan->ch_num;
+        }
+        return false;
+}
+EXPORT_SYMBOL(edma_filter_fn);
+
+static struct platform_device *pdev0, *pdev1;
+
+static const struct platform_device_info edma_dev_info0 = {
+        .name = "edma-dma-engine",
+        .id = 0,
+        .dma_mask = DMA_BIT_MASK(32),
+};
+
+static const struct platform_device_info edma_dev_info1 = {
+        .name = "edma-dma-engine",
+        .id = 1,
+        .dma_mask = DMA_BIT_MASK(32),
+};
+
+static int edma_init(void)
+{
+        int ret = platform_driver_register(&edma_driver);
+
+        if (ret == 0) {
+                pdev0 = platform_device_register_full(&edma_dev_info0);
+                if (IS_ERR(pdev0)) {
+                        platform_driver_unregister(&edma_driver);
+                        ret = PTR_ERR(pdev0);
+                        goto out;
+                }
+        }
+
+        if (EDMA_CTLRS == 2) {
+                pdev1 = platform_device_register_full(&edma_dev_info1);
+                if (IS_ERR(pdev1)) {
+                        platform_driver_unregister(&edma_driver);
+                        platform_device_unregister(pdev0);
+                        ret = PTR_ERR(pdev1);
+                }
+        }
+
+out:
+        return ret;
+}
+subsys_initcall(edma_init);
+
+static void __exit edma_exit(void)
+{
+        platform_device_unregister(pdev0);
+        if (pdev1)
+                platform_device_unregister(pdev1);
+        platform_driver_unregister(&edma_driver);
+}
+module_exit(edma_exit);
+
+MODULE_AUTHOR("Matt Porter <mporter@ti.com>");
+MODULE_DESCRIPTION("TI EDMA DMA engine driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index f11b5b2b1a1c..7d9554cc4976 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -474,8 +474,10 @@ static int imxdma_xfer_desc(struct imxdma_desc *d)
                         slot = i;
                         break;
                 }
-                if (slot < 0)
+                if (slot < 0) {
+                        spin_unlock_irqrestore(&imxdma->lock, flags);
                         return -EBUSY;
+                }
 
                 imxdma->slots_2d[slot].xsr = d->x;
                 imxdma->slots_2d[slot].ysr = d->y;
diff --git a/drivers/dma/ioat/dma_v2.c b/drivers/dma/ioat/dma_v2.c
index 86895760b598..b9d667851445 100644
--- a/drivers/dma/ioat/dma_v2.c
+++ b/drivers/dma/ioat/dma_v2.c
@@ -434,12 +434,11 @@ static struct ioat_ring_ent *ioat2_alloc_ring_ent(struct dma_chan *chan, gfp_t f
                 return NULL;
         memset(hw, 0, sizeof(*hw));
 
-        desc = kmem_cache_alloc(ioat2_cache, flags);
+        desc = kmem_cache_zalloc(ioat2_cache, flags);
         if (!desc) {
                 pci_pool_free(dma->dma_pool, hw, phys);
                 return NULL;
         }
-        memset(desc, 0, sizeof(*desc));
 
         dma_async_tx_descriptor_init(&desc->txd, chan);
         desc->txd.tx_submit = ioat2_tx_submit_unlock;
diff --git a/drivers/dma/ioat/pci.c b/drivers/dma/ioat/pci.c
index 5e3a40f79945..c0573061b45d 100644
--- a/drivers/dma/ioat/pci.c
+++ b/drivers/dma/ioat/pci.c
@@ -40,6 +40,17 @@ MODULE_VERSION(IOAT_DMA_VERSION);
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Intel Corporation");
 
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB0   0x0e20
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB1   0x0e21
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB2   0x0e22
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB3   0x0e23
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB4   0x0e24
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB5   0x0e25
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB6   0x0e26
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB7   0x0e27
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB8   0x0e2e
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB9   0x0e2f
+
 static struct pci_device_id ioat_pci_tbl[] = {
         /* I/OAT v1 platforms */
         { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT) },
@@ -83,6 +94,17 @@ static struct pci_device_id ioat_pci_tbl[] = {
         { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_SNB8) },
         { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_SNB9) },
 
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB0) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB1) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB2) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB3) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB4) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB5) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB6) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB7) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB8) },
+        { PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB9) },
+
         { 0, }
 };
 MODULE_DEVICE_TABLE(pci, ioat_pci_tbl);
diff --git a/drivers/dma/mmp_pdma.c b/drivers/dma/mmp_pdma.c
new file mode 100644
index 000000000000..14da1f403edf
--- /dev/null
+++ b/drivers/dma/mmp_pdma.c
@@ -0,0 +1,875 @@
+/*
+ * Copyright 2012 Marvell International Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/platform_data/mmp_dma.h>
+#include <linux/dmapool.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+
+#include "dmaengine.h"
+
+#define DCSR            0x0000
+#define DALGN           0x00a0
+#define DINT            0x00f0
+#define DDADR           0x0200
+#define DSADR           0x0204
+#define DTADR           0x0208
+#define DCMD            0x020c
+
+#define DCSR_RUN        (1 << 31)       /* Run Bit (read / write) */
+#define DCSR_NODESC     (1 << 30)       /* No-Descriptor Fetch (read / write) */
+#define DCSR_STOPIRQEN  (1 << 29)       /* Stop Interrupt Enable (read / write) */
+#define DCSR_REQPEND    (1 << 8)        /* Request Pending (read-only) */
+#define DCSR_STOPSTATE  (1 << 3)        /* Stop State (read-only) */
+#define DCSR_ENDINTR    (1 << 2)        /* End Interrupt (read / write) */
+#define DCSR_STARTINTR  (1 << 1)        /* Start Interrupt (read / write) */
+#define DCSR_BUSERR     (1 << 0)        /* Bus Error Interrupt (read / write) */
+
+#define DCSR_EORIRQEN   (1 << 28)       /* End of Receive Interrupt Enable (R/W) */
+#define DCSR_EORJMPEN   (1 << 27)       /* Jump to next descriptor on EOR */
+#define DCSR_EORSTOPEN  (1 << 26)       /* STOP on an EOR */
+#define DCSR_SETCMPST   (1 << 25)       /* Set Descriptor Compare Status */
+#define DCSR_CLRCMPST   (1 << 24)       /* Clear Descriptor Compare Status */
+#define DCSR_CMPST      (1 << 10)       /* The Descriptor Compare Status */
+#define DCSR_EORINTR    (1 << 9)        /* The end of Receive */
+
+#define DRCMR_MAPVLD    (1 << 7)        /* Map Valid (read / write) */
+#define DRCMR_CHLNUM    0x1f            /* mask for Channel Number (read / write) */
+
+#define DDADR_DESCADDR  0xfffffff0      /* Address of next descriptor (mask) */
+#define DDADR_STOP      (1 << 0)        /* Stop (read / write) */
+
+#define DCMD_INCSRCADDR (1 << 31)       /* Source Address Increment Setting. */
+#define DCMD_INCTRGADDR (1 << 30)       /* Target Address Increment Setting. */
+#define DCMD_FLOWSRC    (1 << 29)       /* Flow Control by the source. */
+#define DCMD_FLOWTRG    (1 << 28)       /* Flow Control by the target. */
+#define DCMD_STARTIRQEN (1 << 22)       /* Start Interrupt Enable */
+#define DCMD_ENDIRQEN   (1 << 21)       /* End Interrupt Enable */
+#define DCMD_ENDIAN     (1 << 18)       /* Device Endian-ness. */
+#define DCMD_BURST8     (1 << 16)       /* 8 byte burst */
+#define DCMD_BURST16    (2 << 16)       /* 16 byte burst */
+#define DCMD_BURST32    (3 << 16)       /* 32 byte burst */
+#define DCMD_WIDTH1     (1 << 14)       /* 1 byte width */
+#define DCMD_WIDTH2     (2 << 14)       /* 2 byte width (HalfWord) */
+#define DCMD_WIDTH4     (3 << 14)       /* 4 byte width (Word) */
+#define DCMD_LENGTH     0x01fff         /* length mask (max = 8K - 1) */
+
+#define PDMA_ALIGNMENT          3
+#define PDMA_MAX_DESC_BYTES     0x1000
+
+struct mmp_pdma_desc_hw {
+        u32 ddadr;      /* Points to the next descriptor + flags */
+        u32 dsadr;      /* DSADR value for the current transfer */
+        u32 dtadr;      /* DTADR value for the current transfer */
+        u32 dcmd;       /* DCMD value for the current transfer */
+} __aligned(32);
+
+struct mmp_pdma_desc_sw {
+        struct mmp_pdma_desc_hw desc;
+        struct list_head node;
+        struct list_head tx_list;
+        struct dma_async_tx_descriptor async_tx;
+};
+
+struct mmp_pdma_phy;
+
+struct mmp_pdma_chan {
+        struct device *dev;
+        struct dma_chan chan;
+        struct dma_async_tx_descriptor desc;
+        struct mmp_pdma_phy *phy;
+        enum dma_transfer_direction dir;
+
+        /* channel's basic info */
+        struct tasklet_struct tasklet;
+        u32 dcmd;
+        u32 drcmr;
+        u32 dev_addr;
+
+        /* list for desc */
+        spinlock_t desc_lock;           /* Descriptor list lock */
+        struct list_head chain_pending; /* Link descriptors queue for pending */
+        struct list_head chain_running; /* Link descriptors queue for running */
+        bool idle;                      /* channel statue machine */
+
+        struct dma_pool *desc_pool;     /* Descriptors pool */
+};
+
+struct mmp_pdma_phy {
+        int idx;
+        void __iomem *base;
+        struct mmp_pdma_chan *vchan;
+};
+
+struct mmp_pdma_device {
+        int                             dma_channels;
+        void __iomem                    *base;
+        struct device                   *dev;
+        struct dma_device               device;
+        struct mmp_pdma_phy             *phy;
+};
+
+#define tx_to_mmp_pdma_desc(tx) container_of(tx, struct mmp_pdma_desc_sw, async_tx)
+#define to_mmp_pdma_desc(lh) container_of(lh, struct mmp_pdma_desc_sw, node)
+#define to_mmp_pdma_chan(dchan) container_of(dchan, struct mmp_pdma_chan, chan)
+#define to_mmp_pdma_dev(dmadev) container_of(dmadev, struct mmp_pdma_device, device)
+
+static void set_desc(struct mmp_pdma_phy *phy, dma_addr_t addr)
+{
+        u32 reg = (phy->idx << 4) + DDADR;
+
+        writel(addr, phy->base + reg);
+}
+
+static void enable_chan(struct mmp_pdma_phy *phy)
+{
+        u32 reg;
+
+        if (!phy->vchan)
+                return;
+
+        reg = phy->vchan->drcmr;
+        reg = (((reg) < 64) ? 0x0100 : 0x1100) + (((reg) & 0x3f) << 2);
+        writel(DRCMR_MAPVLD | phy->idx, phy->base + reg);
+
+        reg = (phy->idx << 2) + DCSR;
+        writel(readl(phy->base + reg) | DCSR_RUN,
+                                        phy->base + reg);
+}
+
+static void disable_chan(struct mmp_pdma_phy *phy)
+{
+        u32 reg;
+
+        if (phy) {
+                reg = (phy->idx << 2) + DCSR;
+                writel(readl(phy->base + reg) & ~DCSR_RUN,
+                                                phy->base + reg);
+        }
+}
+
+static int clear_chan_irq(struct mmp_pdma_phy *phy)
+{
+        u32 dcsr;
+        u32 dint = readl(phy->base + DINT);
+        u32 reg = (phy->idx << 2) + DCSR;
+
+        if (dint & BIT(phy->idx)) {
+                /* clear irq */
+                dcsr = readl(phy->base + reg);
+                writel(dcsr, phy->base + reg);
+                if ((dcsr & DCSR_BUSERR) && (phy->vchan))
+                        dev_warn(phy->vchan->dev, "DCSR_BUSERR\n");
+                return 0;
+        }
+        return -EAGAIN;
+}
+
+static irqreturn_t mmp_pdma_chan_handler(int irq, void *dev_id)
+{
+        struct mmp_pdma_phy *phy = dev_id;
+
+        if (clear_chan_irq(phy) == 0) {
+                tasklet_schedule(&phy->vchan->tasklet);
+                return IRQ_HANDLED;
+        } else
+                return IRQ_NONE;
+}
+
+static irqreturn_t mmp_pdma_int_handler(int irq, void *dev_id)
+{
+        struct mmp_pdma_device *pdev = dev_id;
+        struct mmp_pdma_phy *phy;
+        u32 dint = readl(pdev->base + DINT);
+        int i, ret;
+        int irq_num = 0;
+
+        while (dint) {
+                i = __ffs(dint);
+                dint &= (dint - 1);
+                phy = &pdev->phy[i];
+                ret = mmp_pdma_chan_handler(irq, phy);
+                if (ret == IRQ_HANDLED)
+                        irq_num++;
+        }
+
+        if (irq_num)
+                return IRQ_HANDLED;
+        else
+                return IRQ_NONE;
+}
+
+/* lookup free phy channel as descending priority */
+static struct mmp_pdma_phy *lookup_phy(struct mmp_pdma_chan *pchan)
+{
+        int prio, i;
+        struct mmp_pdma_device *pdev = to_mmp_pdma_dev(pchan->chan.device);
+        struct mmp_pdma_phy *phy;
+
+        /*
+         * dma channel priorities
+         * ch 0 - 3,  16 - 19  <--> (0)
+         * ch 4 - 7,  20 - 23  <--> (1)
+         * ch 8 - 11, 24 - 27  <--> (2)
+         * ch 12 - 15, 28 - 31  <--> (3)
+         */
+        for (prio = 0; prio <= (((pdev->dma_channels - 1) & 0xf) >> 2); prio++) {
+                for (i = 0; i < pdev->dma_channels; i++) {
+                        if (prio != ((i & 0xf) >> 2))
+                                continue;
+                        phy = &pdev->phy[i];
+                        if (!phy->vchan) {
+                                phy->vchan = pchan;
+                                return phy;
+                        }
+                }
+        }
+
+        return NULL;
+}
+
+/* desc->tx_list ==> pending list */
+static void append_pending_queue(struct mmp_pdma_chan *chan,
+                                        struct mmp_pdma_desc_sw *desc)
+{
+        struct mmp_pdma_desc_sw *tail =
+                                to_mmp_pdma_desc(chan->chain_pending.prev);
+
+        if (list_empty(&chan->chain_pending))
+                goto out_splice;
+
+        /* one irq per queue, even appended */
+        tail->desc.ddadr = desc->async_tx.phys;
+        tail->desc.dcmd &= ~DCMD_ENDIRQEN;
+
+        /* softly link to pending list */
+out_splice:
+        list_splice_tail_init(&desc->tx_list, &chan->chain_pending);
+}
+
+/**
+ * start_pending_queue - transfer any pending transactions
+ * pending list ==> running list
+ */
+static void start_pending_queue(struct mmp_pdma_chan *chan)
+{
+        struct mmp_pdma_desc_sw *desc;
+
+        /* still in running, irq will start the pending list */
+        if (!chan->idle) {
+                dev_dbg(chan->dev, "DMA controller still busy\n");
+                return;
+        }
+
+        if (list_empty(&chan->chain_pending)) {
+                /* chance to re-fetch phy channel with higher prio */
+                if (chan->phy) {
+                        chan->phy->vchan = NULL;
+                        chan->phy = NULL;
+                }
+                dev_dbg(chan->dev, "no pending list\n");
+                return;
+        }
+
+        if (!chan->phy) {
+                chan->phy = lookup_phy(chan);
+                if (!chan->phy) {
+                        dev_dbg(chan->dev, "no free dma channel\n");
+                        return;
+                }
+        }
+
+        /*
+         * pending -> running
+         * reintilize pending list
+         */
+        desc = list_first_entry(&chan->chain_pending,
+                                struct mmp_pdma_desc_sw, node);
+        list_splice_tail_init(&chan->chain_pending, &chan->chain_running);
+
+        /*
+         * Program the descriptor's address into the DMA controller,
+         * then start the DMA transaction
+         */
+        set_desc(chan->phy, desc->async_tx.phys);
+        enable_chan(chan->phy);
+        chan->idle = false;
+}
+
+
+/* desc->tx_list ==> pending list */
+static dma_cookie_t mmp_pdma_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(tx->chan);
+        struct mmp_pdma_desc_sw *desc = tx_to_mmp_pdma_desc(tx);
+        struct mmp_pdma_desc_sw *child;
+        unsigned long flags;
+        dma_cookie_t cookie = -EBUSY;
+
+        spin_lock_irqsave(&chan->desc_lock, flags);
+
+        list_for_each_entry(child, &desc->tx_list, node) {
+                cookie = dma_cookie_assign(&child->async_tx);
+        }
+
+        append_pending_queue(chan, desc);
+
+        spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+        return cookie;
+}
+
+struct mmp_pdma_desc_sw *mmp_pdma_alloc_descriptor(struct mmp_pdma_chan *chan)
+{
+        struct mmp_pdma_desc_sw *desc;
+        dma_addr_t pdesc;
+
+        desc = dma_pool_alloc(chan->desc_pool, GFP_ATOMIC, &pdesc);
+        if (!desc) {
+                dev_err(chan->dev, "out of memory for link descriptor\n");
+                return NULL;
+        }
+
+        memset(desc, 0, sizeof(*desc));
+        INIT_LIST_HEAD(&desc->tx_list);
+        dma_async_tx_descriptor_init(&desc->async_tx, &chan->chan);
+        /* each desc has submit */
+        desc->async_tx.tx_submit = mmp_pdma_tx_submit;
+        desc->async_tx.phys = pdesc;
+
+        return desc;
+}
+
+/**
+ * mmp_pdma_alloc_chan_resources - Allocate resources for DMA channel.
+ *
+ * This function will create a dma pool for descriptor allocation.
+ * Request irq only when channel is requested
+ * Return - The number of allocated descriptors.
+ */
+
+static int mmp_pdma_alloc_chan_resources(struct dma_chan *dchan)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+
+        if (chan->desc_pool)
+                return 1;
+
+        chan->desc_pool =
+                dma_pool_create(dev_name(&dchan->dev->device), chan->dev,
+                                  sizeof(struct mmp_pdma_desc_sw),
+                                  __alignof__(struct mmp_pdma_desc_sw), 0);
+        if (!chan->desc_pool) {
+                dev_err(chan->dev, "unable to allocate descriptor pool\n");
+                return -ENOMEM;
+        }
+        if (chan->phy) {
+                chan->phy->vchan = NULL;
+                chan->phy = NULL;
+        }
+        chan->idle = true;
+        chan->dev_addr = 0;
+        return 1;
+}
+
+static void mmp_pdma_free_desc_list(struct mmp_pdma_chan *chan,
+                                  struct list_head *list)
+{
+        struct mmp_pdma_desc_sw *desc, *_desc;
+
+        list_for_each_entry_safe(desc, _desc, list, node) {
+                list_del(&desc->node);
+                dma_pool_free(chan->desc_pool, desc, desc->async_tx.phys);
+        }
+}
+
+static void mmp_pdma_free_chan_resources(struct dma_chan *dchan)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+        unsigned long flags;
+
+        spin_lock_irqsave(&chan->desc_lock, flags);
+        mmp_pdma_free_desc_list(chan, &chan->chain_pending);
+        mmp_pdma_free_desc_list(chan, &chan->chain_running);
+        spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+        dma_pool_destroy(chan->desc_pool);
+        chan->desc_pool = NULL;
+        chan->idle = true;
+        chan->dev_addr = 0;
+        if (chan->phy) {
+                chan->phy->vchan = NULL;
+                chan->phy = NULL;
+        }
+        return;
+}
+
+static struct dma_async_tx_descriptor *
+mmp_pdma_prep_memcpy(struct dma_chan *dchan,
+        dma_addr_t dma_dst, dma_addr_t dma_src,
+        size_t len, unsigned long flags)
+{
+        struct mmp_pdma_chan *chan;
+        struct mmp_pdma_desc_sw *first = NULL, *prev = NULL, *new;
+        size_t copy = 0;
+
+        if (!dchan)
+                return NULL;
+
+        if (!len)
+                return NULL;
+
+        chan = to_mmp_pdma_chan(dchan);
+
+        if (!chan->dir) {
+                chan->dir = DMA_MEM_TO_MEM;
+                chan->dcmd = DCMD_INCTRGADDR | DCMD_INCSRCADDR;
+                chan->dcmd |= DCMD_BURST32;
+        }
+
+        do {
+                /* Allocate the link descriptor from DMA pool */
+                new = mmp_pdma_alloc_descriptor(chan);
+                if (!new) {
+                        dev_err(chan->dev, "no memory for desc\n");
+                        goto fail;
+                }
+
+                copy = min_t(size_t, len, PDMA_MAX_DESC_BYTES);
+
+                new->desc.dcmd = chan->dcmd | (DCMD_LENGTH & copy);
+                new->desc.dsadr = dma_src;
+                new->desc.dtadr = dma_dst;
+
+                if (!first)
+                        first = new;
+                else
+                        prev->desc.ddadr = new->async_tx.phys;
+
+                new->async_tx.cookie = 0;
+                async_tx_ack(&new->async_tx);
+
+                prev = new;
+                len -= copy;
+
+                if (chan->dir == DMA_MEM_TO_DEV) {
+                        dma_src += copy;
+                } else if (chan->dir == DMA_DEV_TO_MEM) {
+                        dma_dst += copy;
+                } else if (chan->dir == DMA_MEM_TO_MEM) {
+                        dma_src += copy;
+                        dma_dst += copy;
+                }
+
+                /* Insert the link descriptor to the LD ring */
+                list_add_tail(&new->node, &first->tx_list);
+        } while (len);
+
+        first->async_tx.flags = flags; /* client is in control of this ack */
+        first->async_tx.cookie = -EBUSY;
+
+        /* last desc and fire IRQ */
+        new->desc.ddadr = DDADR_STOP;
+        new->desc.dcmd |= DCMD_ENDIRQEN;
+
+        return &first->async_tx;
+
+fail:
+        if (first)
+                mmp_pdma_free_desc_list(chan, &first->tx_list);
+        return NULL;
+}
+
+static struct dma_async_tx_descriptor *
+mmp_pdma_prep_slave_sg(struct dma_chan *dchan, struct scatterlist *sgl,
+                         unsigned int sg_len, enum dma_transfer_direction dir,
+                         unsigned long flags, void *context)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+        struct mmp_pdma_desc_sw *first = NULL, *prev = NULL, *new = NULL;
+        size_t len, avail;
+        struct scatterlist *sg;
+        dma_addr_t addr;
+        int i;
+
+        if ((sgl == NULL) || (sg_len == 0))
+                return NULL;
+
+        for_each_sg(sgl, sg, sg_len, i) {
+                addr = sg_dma_address(sg);
+                avail = sg_dma_len(sgl);
+
+                do {
+                        len = min_t(size_t, avail, PDMA_MAX_DESC_BYTES);
+
+                        /* allocate and populate the descriptor */
+                        new = mmp_pdma_alloc_descriptor(chan);
+                        if (!new) {
+                                dev_err(chan->dev, "no memory for desc\n");
+                                goto fail;
+                        }
+
+                        new->desc.dcmd = chan->dcmd | (DCMD_LENGTH & len);
+                        if (dir == DMA_MEM_TO_DEV) {
+                                new->desc.dsadr = addr;
+                                new->desc.dtadr = chan->dev_addr;
+                        } else {
+                                new->desc.dsadr = chan->dev_addr;
+                                new->desc.dtadr = addr;
+                        }
+
+                        if (!first)
+                                first = new;
+                        else
+                                prev->desc.ddadr = new->async_tx.phys;
+
+                        new->async_tx.cookie = 0;
+                        async_tx_ack(&new->async_tx);
+                        prev = new;
+
+                        /* Insert the link descriptor to the LD ring */
+                        list_add_tail(&new->node, &first->tx_list);
+
+                        /* update metadata */
+                        addr += len;
+                        avail -= len;
+                } while (avail);
+        }
+
+        first->async_tx.cookie = -EBUSY;
+        first->async_tx.flags = flags;
+
+        /* last desc and fire IRQ */
+        new->desc.ddadr = DDADR_STOP;
+        new->desc.dcmd |= DCMD_ENDIRQEN;
+
+        return &first->async_tx;
+
+fail:
+        if (first)
+                mmp_pdma_free_desc_list(chan, &first->tx_list);
+        return NULL;
+}
+
+static int mmp_pdma_control(struct dma_chan *dchan, enum dma_ctrl_cmd cmd,
+                unsigned long arg)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+        struct dma_slave_config *cfg = (void *)arg;
+        unsigned long flags;
+        int ret = 0;
+        u32 maxburst = 0, addr = 0;
+        enum dma_slave_buswidth width = DMA_SLAVE_BUSWIDTH_UNDEFINED;
+
+        if (!dchan)
+                return -EINVAL;
+
+        switch (cmd) {
+        case DMA_TERMINATE_ALL:
+                disable_chan(chan->phy);
+                if (chan->phy) {
+                        chan->phy->vchan = NULL;
+                        chan->phy = NULL;
+                }
+                spin_lock_irqsave(&chan->desc_lock, flags);
+                mmp_pdma_free_desc_list(chan, &chan->chain_pending);
+                mmp_pdma_free_desc_list(chan, &chan->chain_running);
+                spin_unlock_irqrestore(&chan->desc_lock, flags);
+                chan->idle = true;
+                break;
+        case DMA_SLAVE_CONFIG:
+                if (cfg->direction == DMA_DEV_TO_MEM) {
+                        chan->dcmd = DCMD_INCTRGADDR | DCMD_FLOWSRC;
+                        maxburst = cfg->src_maxburst;
+                        width = cfg->src_addr_width;
+                        addr = cfg->src_addr;
+                } else if (cfg->direction == DMA_MEM_TO_DEV) {
+                        chan->dcmd = DCMD_INCSRCADDR | DCMD_FLOWTRG;
+                        maxburst = cfg->dst_maxburst;
+                        width = cfg->dst_addr_width;
+                        addr = cfg->dst_addr;
+                }
+
+                if (width == DMA_SLAVE_BUSWIDTH_1_BYTE)
+                        chan->dcmd |= DCMD_WIDTH1;
+                else if (width == DMA_SLAVE_BUSWIDTH_2_BYTES)
+                        chan->dcmd |= DCMD_WIDTH2;
+                else if (width == DMA_SLAVE_BUSWIDTH_4_BYTES)
+                        chan->dcmd |= DCMD_WIDTH4;
+
+                if (maxburst == 8)
+                        chan->dcmd |= DCMD_BURST8;
+                else if (maxburst == 16)
+                        chan->dcmd |= DCMD_BURST16;
+                else if (maxburst == 32)
+                        chan->dcmd |= DCMD_BURST32;
+
+                if (cfg) {
+                        chan->dir = cfg->direction;
+                        chan->drcmr = cfg->slave_id;
+                }
+                chan->dev_addr = addr;
+                break;
+        default:
+                return -ENOSYS;
+        }
+
+        return ret;
+}
+
+static enum dma_status mmp_pdma_tx_status(struct dma_chan *dchan,
+                        dma_cookie_t cookie, struct dma_tx_state *txstate)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+        enum dma_status ret;
+        unsigned long flags;
+
+        spin_lock_irqsave(&chan->desc_lock, flags);
+        ret = dma_cookie_status(dchan, cookie, txstate);
+        spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+        return ret;
+}
+
+/**
+ * mmp_pdma_issue_pending - Issue the DMA start command
+ * pending list ==> running list
+ */
+static void mmp_pdma_issue_pending(struct dma_chan *dchan)
+{
+        struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+        unsigned long flags;
+
+        spin_lock_irqsave(&chan->desc_lock, flags);
+        start_pending_queue(chan);
+        spin_unlock_irqrestore(&chan->desc_lock, flags);
+}
+
+/*
+ * dma_do_tasklet
+ * Do call back
+ * Start pending list
+ */
+static void dma_do_tasklet(unsigned long data)
+{
+        struct mmp_pdma_chan *chan = (struct mmp_pdma_chan *)data;
+        struct mmp_pdma_desc_sw *desc, *_desc;
+        LIST_HEAD(chain_cleanup);
+        unsigned long flags;
+
+        /* submit pending list; callback for each desc; free desc */
+
+        spin_lock_irqsave(&chan->desc_lock, flags);
+
+        /* update the cookie if we have some descriptors to cleanup */
+        if (!list_empty(&chan->chain_running)) {
+                dma_cookie_t cookie;
+
+                desc = to_mmp_pdma_desc(chan->chain_running.prev);
+                cookie = desc->async_tx.cookie;
+                dma_cookie_complete(&desc->async_tx);
+
+                dev_dbg(chan->dev, "completed_cookie=%d\n", cookie);
+        }
+
+        /*
+         * move the descriptors to a temporary list so we can drop the lock
+         * during the entire cleanup operation
+         */
+        list_splice_tail_init(&chan->chain_running, &chain_cleanup);
+
+        /* the hardware is now idle and ready for more */
+        chan->idle = true;
+
+        /* Start any pending transactions automatically */
+        start_pending_queue(chan);
+        spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+        /* Run the callback for each descriptor, in order */
+        list_for_each_entry_safe(desc, _desc, &chain_cleanup, node) {
+                struct dma_async_tx_descriptor *txd = &desc->async_tx;
+
+                /* Remove from the list of transactions */
+                list_del(&desc->node);
+                /* Run the link descriptor callback function */
+                if (txd->callback)
+                        txd->callback(txd->callback_param);
+
+                dma_pool_free(chan->desc_pool, desc, txd->phys);
+        }
+}
+
+static int __devexit mmp_pdma_remove(struct platform_device *op)
+{
+        struct mmp_pdma_device *pdev = platform_get_drvdata(op);
+
+        dma_async_device_unregister(&pdev->device);
+        return 0;
+}
+
+static int __devinit mmp_pdma_chan_init(struct mmp_pdma_device *pdev,
+                                                        int idx, int irq)
+{
+        struct mmp_pdma_phy *phy  = &pdev->phy[idx];
+        struct mmp_pdma_chan *chan;
+        int ret;
+
+        chan = devm_kzalloc(pdev->dev,
+                        sizeof(struct mmp_pdma_chan), GFP_KERNEL);
+        if (chan == NULL)
+                return -ENOMEM;
+
+        phy->idx = idx;
+        phy->base = pdev->base;
+
+        if (irq) {
+                ret = devm_request_irq(pdev->dev, irq,
+                        mmp_pdma_chan_handler, IRQF_DISABLED, "pdma", phy);
+                if (ret) {
+                        dev_err(pdev->dev, "channel request irq fail!\n");
+                        return ret;
+                }
+        }
+
+        spin_lock_init(&chan->desc_lock);
+        chan->dev = pdev->dev;
+        chan->chan.device = &pdev->device;
+        tasklet_init(&chan->tasklet, dma_do_tasklet, (unsigned long)chan);
+        INIT_LIST_HEAD(&chan->chain_pending);
+        INIT_LIST_HEAD(&chan->chain_running);
+
+        /* register virt channel to dma engine */
+        list_add_tail(&chan->chan.device_node,
+                        &pdev->device.channels);
+
+        return 0;
+}
+
+static struct of_device_id mmp_pdma_dt_ids[] = {
+        { .compatible = "marvell,pdma-1.0", },
+        {}
+};
+MODULE_DEVICE_TABLE(of, mmp_pdma_dt_ids);
+
+static int __devinit mmp_pdma_probe(struct platform_device *op)
+{
+        struct mmp_pdma_device *pdev;
+        const struct of_device_id *of_id;
+        struct mmp_dma_platdata *pdata = dev_get_platdata(&op->dev);
+        struct resource *iores;
+        int i, ret, irq = 0;
+        int dma_channels = 0, irq_num = 0;
+
+        pdev = devm_kzalloc(&op->dev, sizeof(*pdev), GFP_KERNEL);
+        if (!pdev)
+                return -ENOMEM;
+        pdev->dev = &op->dev;
+
+        iores = platform_get_resource(op, IORESOURCE_MEM, 0);
+        if (!iores)
+                return -EINVAL;
+
+        pdev->base = devm_request_and_ioremap(pdev->dev, iores);
+        if (!pdev->base)
+                return -EADDRNOTAVAIL;
+
+        of_id = of_match_device(mmp_pdma_dt_ids, pdev->dev);
+        if (of_id)
+                of_property_read_u32(pdev->dev->of_node,
+                                "#dma-channels", &dma_channels);
+        else if (pdata && pdata->dma_channels)
+                dma_channels = pdata->dma_channels;
+        else
+                dma_channels = 32;      /* default 32 channel */
+        pdev->dma_channels = dma_channels;
+
+        for (i = 0; i < dma_channels; i++) {
+                if (platform_get_irq(op, i) > 0)
+                        irq_num++;
+        }
+
+        pdev->phy = devm_kzalloc(pdev->dev,
+                dma_channels * sizeof(struct mmp_pdma_chan), GFP_KERNEL);
+        if (pdev->phy == NULL)
+                return -ENOMEM;
+
+        INIT_LIST_HEAD(&pdev->device.channels);
+
+        if (irq_num != dma_channels) {
+                /* all chan share one irq, demux inside */
+                irq = platform_get_irq(op, 0);
+                ret = devm_request_irq(pdev->dev, irq,
+                        mmp_pdma_int_handler, IRQF_DISABLED, "pdma", pdev);
+                if (ret)
+                        return ret;
+        }
+
+        for (i = 0; i < dma_channels; i++) {
+                irq = (irq_num != dma_channels) ? 0 : platform_get_irq(op, i);
+                ret = mmp_pdma_chan_init(pdev, i, irq);
+                if (ret)
+                        return ret;
+        }
+
+        dma_cap_set(DMA_SLAVE, pdev->device.cap_mask);
+        dma_cap_set(DMA_MEMCPY, pdev->device.cap_mask);
+        dma_cap_set(DMA_SLAVE, pdev->device.cap_mask);
+        pdev->device.dev = &op->dev;
+        pdev->device.device_alloc_chan_resources = mmp_pdma_alloc_chan_resources;
+        pdev->device.device_free_chan_resources = mmp_pdma_free_chan_resources;
+        pdev->device.device_tx_status = mmp_pdma_tx_status;
+        pdev->device.device_prep_dma_memcpy = mmp_pdma_prep_memcpy;
+        pdev->device.device_prep_slave_sg = mmp_pdma_prep_slave_sg;
+        pdev->device.device_issue_pending = mmp_pdma_issue_pending;
+        pdev->device.device_control = mmp_pdma_control;
+        pdev->device.copy_align = PDMA_ALIGNMENT;
+
+        if (pdev->dev->coherent_dma_mask)
+                dma_set_mask(pdev->dev, pdev->dev->coherent_dma_mask);
+        else
+                dma_set_mask(pdev->dev, DMA_BIT_MASK(64));
+
+        ret = dma_async_device_register(&pdev->device);
+        if (ret) {
+                dev_err(pdev->device.dev, "unable to register\n");
+                return ret;
+        }
+
+        dev_info(pdev->device.dev, "initialized\n");
+        return 0;
+}
+
+static const struct platform_device_id mmp_pdma_id_table[] = {
+        { "mmp-pdma", },
+        { },
+};
+
+static struct platform_driver mmp_pdma_driver = {
+        .driver         = {
+                .name   = "mmp-pdma",
+                .owner  = THIS_MODULE,
+                .of_match_table = mmp_pdma_dt_ids,
+        },
+        .id_table       = mmp_pdma_id_table,
+        .probe          = mmp_pdma_probe,
+        .remove         = __devexit_p(mmp_pdma_remove),
+};
+
+module_platform_driver(mmp_pdma_driver);
+
+MODULE_DESCRIPTION("MARVELL MMP Periphera DMA Driver");
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index 6d9c82e891d7..f3e8d71bcbc7 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -20,6 +20,7 @@
 #include <linux/device.h>
 #include <mach/regs-icu.h>
 #include <linux/platform_data/dma-mmp_tdma.h>
+#include <linux/of_device.h>
 
 #include "dmaengine.h"
 
@@ -127,7 +128,6 @@ struct mmp_tdma_device {
         void __iomem                    *base;
         struct dma_device               device;
         struct mmp_tdma_chan            *tdmac[TDMA_CHANNEL_NUM];
-        int                             irq;
 };
 
 #define to_mmp_tdma_chan(dchan) container_of(dchan, struct mmp_tdma_chan, chan)
@@ -492,7 +492,7 @@ static int __devinit mmp_tdma_chan_init(struct mmp_tdma_device *tdev,
                 return -ENOMEM;
         }
         if (irq)
-                tdmac->irq = irq + idx;
+                tdmac->irq = irq;
         tdmac->dev         = tdev->dev;
         tdmac->chan.device = &tdev->device;
         tdmac->idx         = idx;
@@ -505,34 +505,43 @@ static int __devinit mmp_tdma_chan_init(struct mmp_tdma_device *tdev,
         /* add the channel to tdma_chan list */
         list_add_tail(&tdmac->chan.device_node,
                         &tdev->device.channels);
-
         return 0;
 }
 
+static struct of_device_id mmp_tdma_dt_ids[] = {
+        { .compatible = "marvell,adma-1.0", .data = (void *)MMP_AUD_TDMA},
+        { .compatible = "marvell,pxa910-squ", .data = (void *)PXA910_SQU},
+        {}
+};
+MODULE_DEVICE_TABLE(of, mmp_tdma_dt_ids);
+
 static int __devinit mmp_tdma_probe(struct platform_device *pdev)
 {
-        const struct platform_device_id *id = platform_get_device_id(pdev);
-        enum mmp_tdma_type type = id->driver_data;
+        enum mmp_tdma_type type;
+        const struct of_device_id *of_id;
         struct mmp_tdma_device *tdev;
         struct resource *iores;
         int i, ret;
-        int irq = 0;
+        int irq = 0, irq_num = 0;
         int chan_num = TDMA_CHANNEL_NUM;
 
+        of_id = of_match_device(mmp_tdma_dt_ids, &pdev->dev);
+        if (of_id)
+                type = (enum mmp_tdma_type) of_id->data;
+        else
+                type = platform_get_device_id(pdev)->driver_data;
+
         /* always have couple channels */
         tdev = devm_kzalloc(&pdev->dev, sizeof(*tdev), GFP_KERNEL);
         if (!tdev)
                 return -ENOMEM;
 
         tdev->dev = &pdev->dev;
-        iores = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-        if (!iores)
-                return -EINVAL;
 
-        if (resource_size(iores) != chan_num)
-                tdev->irq = iores->start;
-        else
-                irq = iores->start;
+        for (i = 0; i < chan_num; i++) {
+                if (platform_get_irq(pdev, i) > 0)
+                        irq_num++;
+        }
 
         iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (!iores)
@@ -542,25 +551,26 @@ static int __devinit mmp_tdma_probe(struct platform_device *pdev)
         if (!tdev->base)
                 return -EADDRNOTAVAIL;
 
-        if (tdev->irq) {
-                ret = devm_request_irq(&pdev->dev, tdev->irq,
+        INIT_LIST_HEAD(&tdev->device.channels);
+
+        if (irq_num != chan_num) {
+                irq = platform_get_irq(pdev, 0);
+                ret = devm_request_irq(&pdev->dev, irq,
                         mmp_tdma_int_handler, IRQF_DISABLED, "tdma", tdev);
                 if (ret)
                         return ret;
         }
 
-        dma_cap_set(DMA_SLAVE, tdev->device.cap_mask);
-        dma_cap_set(DMA_CYCLIC, tdev->device.cap_mask);
-
-        INIT_LIST_HEAD(&tdev->device.channels);
-
         /* initialize channel parameters */
         for (i = 0; i < chan_num; i++) {
+                irq = (irq_num != chan_num) ? 0 : platform_get_irq(pdev, i);
                 ret = mmp_tdma_chan_init(tdev, i, irq, type);
                 if (ret)
                         return ret;
         }
 
+        dma_cap_set(DMA_SLAVE, tdev->device.cap_mask);
+        dma_cap_set(DMA_CYCLIC, tdev->device.cap_mask);
         tdev->device.dev = &pdev->dev;
         tdev->device.device_alloc_chan_resources =
                                         mmp_tdma_alloc_chan_resources;
@@ -595,6 +605,7 @@ static struct platform_driver mmp_tdma_driver = {
         .driver         = {
                 .name   = "mmp-tdma",
                 .owner  = THIS_MODULE,
+                .of_match_table = mmp_tdma_dt_ids,
         },
         .id_table       = mmp_tdma_id_table,
         .probe          = mmp_tdma_probe,
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 734a4eb84d65..9f02e794b12b 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -101,7 +101,8 @@ struct mxs_dma_ccw {
         u32             pio_words[MXS_PIO_WORDS];
 };
 
-#define NUM_CCW (int)(PAGE_SIZE / sizeof(struct mxs_dma_ccw))
+#define CCW_BLOCK_SIZE  (4 * PAGE_SIZE)
+#define NUM_CCW (int)(CCW_BLOCK_SIZE / sizeof(struct mxs_dma_ccw))
 
 struct mxs_dma_chan {
         struct mxs_dma_engine           *mxs_dma;
@@ -354,14 +355,15 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 
         mxs_chan->chan_irq = data->chan_irq;
 
-        mxs_chan->ccw = dma_alloc_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
-                                &mxs_chan->ccw_phys, GFP_KERNEL);
+        mxs_chan->ccw = dma_alloc_coherent(mxs_dma->dma_device.dev,
+                                CCW_BLOCK_SIZE, &mxs_chan->ccw_phys,
+                                GFP_KERNEL);
         if (!mxs_chan->ccw) {
                 ret = -ENOMEM;
                 goto err_alloc;
         }
 
-        memset(mxs_chan->ccw, 0, PAGE_SIZE);
+        memset(mxs_chan->ccw, 0, CCW_BLOCK_SIZE);
 
         if (mxs_chan->chan_irq != NO_IRQ) {
                 ret = request_irq(mxs_chan->chan_irq, mxs_dma_int_handler,
@@ -387,7 +389,7 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 err_clk:
         free_irq(mxs_chan->chan_irq, mxs_dma);
 err_irq:
-        dma_free_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
+        dma_free_coherent(mxs_dma->dma_device.dev, CCW_BLOCK_SIZE,
                         mxs_chan->ccw, mxs_chan->ccw_phys);
 err_alloc:
         return ret;
@@ -402,7 +404,7 @@ static void mxs_dma_free_chan_resources(struct dma_chan *chan)
 
         free_irq(mxs_chan->chan_irq, mxs_dma);
 
-        dma_free_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
+        dma_free_coherent(mxs_dma->dma_device.dev, CCW_BLOCK_SIZE,
                         mxs_chan->ccw, mxs_chan->ccw_phys);
 
         clk_disable_unprepare(mxs_dma->clk);
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 169c0dbd71ae..665668b6f2b1 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -23,7 +23,6 @@
 #include <linux/dmaengine.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/pl330.h>
-#include <linux/pm_runtime.h>
 #include <linux/scatterlist.h>
 #include <linux/of.h>
 
@@ -586,8 +585,6 @@ struct dma_pl330_dmac {
 
         /* Peripheral channels connected to this DMAC */
         struct dma_pl330_chan *peripherals; /* keep at end */
-
-        struct clk *clk;
 };
 
 struct dma_pl330_desc {
@@ -2395,7 +2392,7 @@ static int pl330_alloc_chan_resources(struct dma_chan *chan)
         pch->pl330_chid = pl330_request_channel(&pdmac->pif);
         if (!pch->pl330_chid) {
                 spin_unlock_irqrestore(&pch->lock, flags);
-                return 0;
+                return -ENOMEM;
         }
 
         tasklet_init(&pch->task, pl330_tasklet, (unsigned long) pch);
@@ -2889,29 +2886,17 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
                 goto probe_err1;
         }
 
-        pdmac->clk = clk_get(&adev->dev, "dma");
-        if (IS_ERR(pdmac->clk)) {
-                dev_err(&adev->dev, "Cannot get operation clock.\n");
-                ret = -EINVAL;
-                goto probe_err2;
-        }
-
         amba_set_drvdata(adev, pdmac);
 
-#ifndef CONFIG_PM_RUNTIME
-        /* enable dma clk */
-        clk_enable(pdmac->clk);
-#endif
-
         irq = adev->irq[0];
         ret = request_irq(irq, pl330_irq_handler, 0,
                         dev_name(&adev->dev), pi);
         if (ret)
-                goto probe_err3;
+                goto probe_err2;
 
         ret = pl330_add(pi);
         if (ret)
-                goto probe_err4;
+                goto probe_err3;
 
         INIT_LIST_HEAD(&pdmac->desc_pool);
         spin_lock_init(&pdmac->pool_lock);
@@ -2933,7 +2918,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
         if (!pdmac->peripherals) {
                 ret = -ENOMEM;
                 dev_err(&adev->dev, "unable to allocate pdmac->peripherals\n");
-                goto probe_err5;
+                goto probe_err4;
         }
 
         for (i = 0; i < num_chan; i++) {
@@ -2961,6 +2946,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
                 if (pi->pcfg.num_peri) {
                         dma_cap_set(DMA_SLAVE, pd->cap_mask);
                         dma_cap_set(DMA_CYCLIC, pd->cap_mask);
+                        dma_cap_set(DMA_PRIVATE, pd->cap_mask);
                 }
         }
 
@@ -2976,7 +2962,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
         ret = dma_async_device_register(pd);
         if (ret) {
                 dev_err(&adev->dev, "unable to register DMAC\n");
-                goto probe_err5;
+                goto probe_err4;
         }
 
         dev_info(&adev->dev,
@@ -2989,15 +2975,10 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 
         return 0;
 
-probe_err5:
-        pl330_del(pi);
 probe_err4:
-        free_irq(irq, pi);
+        pl330_del(pi);
 probe_err3:
-#ifndef CONFIG_PM_RUNTIME
-        clk_disable(pdmac->clk);
-#endif
-        clk_put(pdmac->clk);
+        free_irq(irq, pi);
 probe_err2:
         iounmap(pi->base);
 probe_err1:
@@ -3044,10 +3025,6 @@ static int __devexit pl330_remove(struct amba_device *adev)
         res = &adev->res;
         release_mem_region(res->start, resource_size(res));
 
-#ifndef CONFIG_PM_RUNTIME
-        clk_disable(pdmac->clk);
-#endif
-
         kfree(pdmac);
 
         return 0;
@@ -3063,49 +3040,10 @@ static struct amba_id pl330_ids[] = {
 
 MODULE_DEVICE_TABLE(amba, pl330_ids);
 
-#ifdef CONFIG_PM_RUNTIME
-static int pl330_runtime_suspend(struct device *dev)
-{
-        struct dma_pl330_dmac *pdmac = dev_get_drvdata(dev);
-
-        if (!pdmac) {
-                dev_err(dev, "failed to get dmac\n");
-                return -ENODEV;
-        }
-
-        clk_disable(pdmac->clk);
-
-        return 0;
-}
-
-static int pl330_runtime_resume(struct device *dev)
-{
-        struct dma_pl330_dmac *pdmac = dev_get_drvdata(dev);
-
-        if (!pdmac) {
-                dev_err(dev, "failed to get dmac\n");
-                return -ENODEV;
-        }
-
-        clk_enable(pdmac->clk);
-
-        return 0;
-}
-#else
-#define pl330_runtime_suspend   NULL
-#define pl330_runtime_resume    NULL
-#endif /* CONFIG_PM_RUNTIME */
-
-static const struct dev_pm_ops pl330_pm_ops = {
-        .runtime_suspend = pl330_runtime_suspend,
-        .runtime_resume = pl330_runtime_resume,
-};
-
 static struct amba_driver pl330_driver = {
         .drv = {
                 .owner = THIS_MODULE,
                 .name = "dma-pl330",
-                .pm = &pl330_pm_ops,
         },
         .id_table = pl330_ids,
         .probe = pl330_probe,
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index 3eed8b35b0f1..d451caace806 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -109,7 +109,7 @@ static void sirfsoc_dma_execute(struct sirfsoc_dma_chan *schan)
         sdesc = list_first_entry(&schan->queued, struct sirfsoc_dma_desc,
                 node);
         /* Move the first queued descriptor to active list */
-        list_move_tail(&schan->queued, &schan->active);
+        list_move_tail(&sdesc->node, &schan->active);
 
         /* Start the DMA transfer */
         writel_relaxed(sdesc->width, sdma->base + SIRFSOC_DMA_WIDTH_0 +
@@ -428,7 +428,7 @@ static struct dma_async_tx_descriptor *sirfsoc_dma_prep_interleaved(
         unsigned long iflags;
         int ret;
 
-        if ((xt->dir != DMA_MEM_TO_DEV) || (xt->dir != DMA_DEV_TO_MEM)) {
+        if ((xt->dir != DMA_MEM_TO_DEV) && (xt->dir != DMA_DEV_TO_MEM)) {
                 ret = -EINVAL;
                 goto err_dir;
         }
@@ -570,21 +570,19 @@ static int __devinit sirfsoc_dma_probe(struct platform_device *op)
 
         if (of_property_read_u32(dn, "cell-index", &id)) {
                 dev_err(dev, "Fail to get DMAC index\n");
-                ret = -ENODEV;
-                goto free_mem;
+                return -ENODEV;
         }
 
         sdma->irq = irq_of_parse_and_map(dn, 0);
         if (sdma->irq == NO_IRQ) {
                 dev_err(dev, "Error mapping IRQ!\n");
-                ret = -EINVAL;
-                goto free_mem;
+                return -EINVAL;
         }
 
         ret = of_address_to_resource(dn, 0, &res);
         if (ret) {
                 dev_err(dev, "Error parsing memory region!\n");
-                goto free_mem;
+                goto irq_dispose;
         }
 
         regs_start = res.start;
@@ -597,12 +595,11 @@ static int __devinit sirfsoc_dma_probe(struct platform_device *op)
                 goto irq_dispose;
         }
 
-        ret = devm_request_irq(dev, sdma->irq, &sirfsoc_dma_irq, 0, DRV_NAME,
-                sdma);
+        ret = request_irq(sdma->irq, &sirfsoc_dma_irq, 0, DRV_NAME, sdma);
         if (ret) {
                 dev_err(dev, "Error requesting IRQ!\n");
                 ret = -EINVAL;
-                goto unmap_mem;
+                goto irq_dispose;
         }
 
         dma = &sdma->dma;
@@ -652,13 +649,9 @@ static int __devinit sirfsoc_dma_probe(struct platform_device *op)
         return 0;
 
 free_irq:
-        devm_free_irq(dev, sdma->irq, sdma);
+        free_irq(sdma->irq, sdma);
 irq_dispose:
         irq_dispose_mapping(sdma->irq);
-unmap_mem:
-        iounmap(sdma->base);
-free_mem:
-        devm_kfree(dev, sdma);
         return ret;
 }
 
@@ -668,10 +661,8 @@ static int __devexit sirfsoc_dma_remove(struct platform_device *op)
         struct sirfsoc_dma *sdma = dev_get_drvdata(dev);
 
         dma_async_device_unregister(&sdma->dma);
-        devm_free_irq(dev, sdma->irq, sdma);
+        free_irq(sdma->irq, sdma);
         irq_dispose_mapping(sdma->irq);
-        iounmap(sdma->base);
-        devm_kfree(dev, sdma);
         return 0;
 }
 
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index eee8d9b9a20b..ae55091c2272 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -2921,19 +2921,23 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
         struct d40_base *base = NULL;
         int num_log_chans = 0;
         int num_phy_chans;
+        int clk_ret = -EINVAL;
         int i;
         u32 pid;
         u32 cid;
         u8 rev;
 
         clk = clk_get(&pdev->dev, NULL);
-
         if (IS_ERR(clk)) {
                 d40_err(&pdev->dev, "No matching clock found\n");
                 goto failure;
         }
 
-        clk_enable(clk);
+        clk_ret = clk_prepare_enable(clk);
+        if (clk_ret) {
+                d40_err(&pdev->dev, "Failed to prepare/enable clock\n");
+                goto failure;
+        }
 
         /* Get IO for DMAC base address */
         res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "base");
@@ -3063,10 +3067,10 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
         return base;
 
 failure:
-        if (!IS_ERR(clk)) {
-                clk_disable(clk);
+        if (!clk_ret)
+                clk_disable_unprepare(clk);
+        if (!IS_ERR(clk))
                 clk_put(clk);
-        }
         if (virtbase)
                 iounmap(virtbase);
         if (res)
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
index 45fbeed1c1a5..528c62dd4b00 100644
--- a/drivers/dma/tegra20-apb-dma.c
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -169,6 +169,7 @@ typedef void (*dma_isr_handler)(struct tegra_dma_channel *tdc,
 /* tegra_dma_channel: Channel specific information */
 struct tegra_dma_channel {
         struct dma_chan         dma_chan;
+        char                    name[30];
         bool                    config_init;
         int                     id;
         int                     irq;
@@ -475,8 +476,7 @@ static void tegra_dma_abort_all(struct tegra_dma_channel *tdc)
         while (!list_empty(&tdc->pending_sg_req)) {
                 sgreq = list_first_entry(&tdc->pending_sg_req,
                                                 typeof(*sgreq), node);
-                list_del(&sgreq->node);
-                list_add_tail(&sgreq->node, &tdc->free_sg_req);
+                list_move_tail(&sgreq->node, &tdc->free_sg_req);
                 if (sgreq->last_sg) {
                         dma_desc = sgreq->dma_desc;
                         dma_desc->dma_status = DMA_ERROR;
@@ -570,8 +570,7 @@ static void handle_cont_sngl_cycle_dma_done(struct tegra_dma_channel *tdc,
 
         /* If not last req then put at end of pending list */
         if (!list_is_last(&sgreq->node, &tdc->pending_sg_req)) {
-                list_del(&sgreq->node);
-                list_add_tail(&sgreq->node, &tdc->pending_sg_req);
+                list_move_tail(&sgreq->node, &tdc->pending_sg_req);
                 sgreq->configured = false;
                 st = handle_continuous_head_request(tdc, sgreq, to_terminate);
                 if (!st)
@@ -1284,7 +1283,6 @@ static int __devinit tegra_dma_probe(struct platform_device *pdev)
         INIT_LIST_HEAD(&tdma->dma_dev.channels);
         for (i = 0; i < cdata->nr_channels; i++) {
                 struct tegra_dma_channel *tdc = &tdma->channels[i];
-                char irq_name[30];
 
                 tdc->chan_base_offset = TEGRA_APBDMA_CHANNEL_BASE_ADD_OFFSET +
                                         i * TEGRA_APBDMA_CHANNEL_REGISTER_SIZE;
@@ -1296,9 +1294,9 @@ static int __devinit tegra_dma_probe(struct platform_device *pdev)
                         goto err_irq;
                 }
                 tdc->irq = res->start;
-                snprintf(irq_name, sizeof(irq_name), "apbdma.%d", i);
+                snprintf(tdc->name, sizeof(tdc->name), "apbdma.%d", i);
                 ret = devm_request_irq(&pdev->dev, tdc->irq,
-                                tegra_dma_isr, 0, irq_name, tdc);
+                                tegra_dma_isr, 0, tdc->name, tdc);
                 if (ret) {
                         dev_err(&pdev->dev,
                                 "request_irq failed with err %d channel %d\n",
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 5a297a26211d..cc8e7c78a23c 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -170,8 +170,11 @@ static int __amd64_set_scrub_rate(struct pci_dev *ctl, u32 new_bw, u32 min_rate)
          * memory controller and apply to register. Search for the first
          * bandwidth entry that is greater or equal than the setting requested
          * and program that. If at last entry, turn off DRAM scrubbing.
+         *
+         * If no suitable bandwidth is found, turn off DRAM scrubbing entirely
+         * by falling back to the last element in scrubrates[].
          */
-        for (i = 0; i < ARRAY_SIZE(scrubrates); i++) {
+        for (i = 0; i < ARRAY_SIZE(scrubrates) - 1; i++) {
                 /*
                  * skip scrub rates which aren't recommended
                  * (see F10 BKDG, F3x58)
@@ -181,12 +184,6 @@ static int __amd64_set_scrub_rate(struct pci_dev *ctl, u32 new_bw, u32 min_rate)
 
                 if (scrubrates[i].bandwidth <= new_bw)
                         break;
-
-                /*
-                 * if no suitable bandwidth found, turn off DRAM scrubbing
-                 * entirely by falling back to the last element in the
-                 * scrubrates array.
-                 */
         }
 
         scrubval = scrubrates[i].scrubval;
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
index 725eb5aa8d8c..e87196f6d2d2 100644
--- a/drivers/extcon/extcon-adc-jack.c
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -14,6 +14,7 @@
  *
  */
 
+#include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
@@ -161,13 +162,12 @@ static int __devinit adc_jack_probe(struct platform_device *pdev)
         err = request_any_context_irq(data->irq, adc_jack_irq_thread,
                         pdata->irq_flags, pdata->name, data);
 
-        if (err) {
+        if (err < 0) {
                 dev_err(&pdev->dev, "error: irq %d\n", data->irq);
-                err = -EINVAL;
                 goto err_irq;
         }
 
-        goto out;
+        return 0;
 
 err_irq:
         extcon_dev_unregister(&data->edev);
@@ -196,3 +196,7 @@ static struct platform_driver adc_jack_driver = {
 };
 
 module_platform_driver(adc_jack_driver);
+
+MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
+MODULE_DESCRIPTION("ADC Jack extcon driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/extcon/extcon-class.c b/drivers/extcon/extcon-class.c
index 946a3188b2b7..d398821097f3 100644
--- a/drivers/extcon/extcon-class.c
+++ b/drivers/extcon/extcon-class.c
@@ -41,7 +41,7 @@
  * every single port-type of the following cable names. Please choose cable
  * names that are actually used in your extcon device.
  */
-const char *extcon_cable_name[] = {
+const char extcon_cable_name[][CABLE_NAME_MAX + 1] = {
         [EXTCON_USB]            = "USB",
         [EXTCON_USB_HOST]       = "USB-Host",
         [EXTCON_TA]             = "TA",
@@ -62,8 +62,6 @@ const char *extcon_cable_name[] = {
         [EXTCON_VIDEO_IN]       = "Video-in",
         [EXTCON_VIDEO_OUT]      = "Video-out",
         [EXTCON_MECHANICAL]     = "Mechanical",
-
-        NULL,
 };
 
 static struct class *extcon_class;
@@ -91,17 +89,13 @@ static int check_mutually_exclusive(struct extcon_dev *edev, u32 new_state)
                 return 0;
 
         for (i = 0; edev->mutually_exclusive[i]; i++) {
-                int count = 0, j;
+                int weight;
                 u32 correspondants = new_state & edev->mutually_exclusive[i];
-                u32 exp = 1;
-
-                for (j = 0; j < 32; j++) {
-                        if (exp & correspondants)
-                                count++;
-                        if (count > 1)
-                                return i + 1;
-                        exp <<= 1;
-                }
+
+                /* calculate the total number of bits set */
+                weight = hweight32(correspondants);
+                if (weight > 1)
+                        return i + 1;
         }
 
         return 0;
@@ -362,7 +356,7 @@ int extcon_get_cable_state(struct extcon_dev *edev, const char *cable_name)
 EXPORT_SYMBOL_GPL(extcon_get_cable_state);
 
 /**
- * extcon_get_cable_state_() - Set the status of a specific cable.
+ * extcon_set_cable_state_() - Set the status of a specific cable.
  * @edev:       the extcon device that has the cable.
  * @index:      cable index that can be retrieved by extcon_find_cable_index().
  * @cable_state:        the new cable status. The default semantics is
@@ -382,7 +376,7 @@ int extcon_set_cable_state_(struct extcon_dev *edev,
 EXPORT_SYMBOL_GPL(extcon_set_cable_state_);
 
 /**
- * extcon_get_cable_state() - Set the status of a specific cable.
+ * extcon_set_cable_state() - Set the status of a specific cable.
  * @edev:       the extcon device that has the cable.
  * @cable_name: cable name.
  * @cable_state:        the new cable status. The default semantics is
@@ -447,6 +441,8 @@ static int _call_per_cable(struct notifier_block *nb, unsigned long val,
  *                            extcon device.
  * @obj:        an empty extcon_specific_cable_nb object to be returned.
  * @extcon_name:        the name of extcon device.
+ *                      if NULL, extcon_register_interest will register
+ *                      every cable with the target cable_name given.
  * @cable_name:         the target cable name.
  * @nb:         the notifier block to get notified.
  *
@@ -466,22 +462,44 @@ int extcon_register_interest(struct extcon_specific_cable_nb *obj,
                              const char *extcon_name, const char *cable_name,
                              struct notifier_block *nb)
 {
-        if (!obj || !extcon_name || !cable_name || !nb)
+        if (!obj || !cable_name || !nb)
                 return -EINVAL;
 
-        obj->edev = extcon_get_extcon_dev(extcon_name);
-        if (!obj->edev)
-                return -ENODEV;
+        if (extcon_name) {
+                obj->edev = extcon_get_extcon_dev(extcon_name);
+                if (!obj->edev)
+                        return -ENODEV;
 
-        obj->cable_index = extcon_find_cable_index(obj->edev, cable_name);
-        if (obj->cable_index < 0)
-                return -ENODEV;
+                obj->cable_index = extcon_find_cable_index(obj->edev, cable_name);
+                if (obj->cable_index < 0)
+                        return -ENODEV;
+
+                obj->user_nb = nb;
 
-        obj->user_nb = nb;
+                obj->internal_nb.notifier_call = _call_per_cable;
 
-        obj->internal_nb.notifier_call = _call_per_cable;
+                return raw_notifier_chain_register(&obj->edev->nh, &obj->internal_nb);
+        } else {
+                struct class_dev_iter iter;
+                struct extcon_dev *extd;
+                struct device *dev;
+
+                if (!extcon_class)
+                        return -ENODEV;
+                class_dev_iter_init(&iter, extcon_class, NULL, NULL);
+                while ((dev = class_dev_iter_next(&iter))) {
+                        extd = (struct extcon_dev *)dev_get_drvdata(dev);
+
+                        if (extcon_find_cable_index(extd, cable_name) < 0)
+                                continue;
+
+                        class_dev_iter_exit(&iter);
+                        return extcon_register_interest(obj, extd->name,
+                                                cable_name, nb);
+                }
 
-        return raw_notifier_chain_register(&obj->edev->nh, &obj->internal_nb);
+                return -ENODEV;
+        }
 }
 
 /**
@@ -551,43 +569,9 @@ static int create_extcon_class(void)
         return 0;
 }
 
-static void extcon_cleanup(struct extcon_dev *edev, bool skip)
-{
-        mutex_lock(&extcon_dev_list_lock);
-        list_del(&edev->entry);
-        mutex_unlock(&extcon_dev_list_lock);
-
-        if (!skip && get_device(edev->dev)) {
-                int index;
-
-                if (edev->mutually_exclusive && edev->max_supported) {
-                        for (index = 0; edev->mutually_exclusive[index];
-                             index++)
-                                kfree(edev->d_attrs_muex[index].attr.name);
-                        kfree(edev->d_attrs_muex);
-                        kfree(edev->attrs_muex);
-                }
-
-                for (index = 0; index < edev->max_supported; index++)
-                        kfree(edev->cables[index].attr_g.name);
-
-                if (edev->max_supported) {
-                        kfree(edev->extcon_dev_type.groups);
-                        kfree(edev->cables);
-                }
-
-                device_unregister(edev->dev);
-                put_device(edev->dev);
-        }
-
-        kfree(edev->dev);
-}
-
 static void extcon_dev_release(struct device *dev)
 {
-        struct extcon_dev *edev = (struct extcon_dev *) dev_get_drvdata(dev);
-
-        extcon_cleanup(edev, true);
+        kfree(dev);
 }
 
 static const char *muex_name = "mutually_exclusive";
@@ -813,7 +797,40 @@ EXPORT_SYMBOL_GPL(extcon_dev_register);
  */
 void extcon_dev_unregister(struct extcon_dev *edev)
 {
-        extcon_cleanup(edev, false);
+        int index;
+
+        mutex_lock(&extcon_dev_list_lock);
+        list_del(&edev->entry);
+        mutex_unlock(&extcon_dev_list_lock);
+
+        if (IS_ERR_OR_NULL(get_device(edev->dev))) {
+                dev_err(edev->dev, "Failed to unregister extcon_dev (%s)\n",
+                                dev_name(edev->dev));
+                return;
+        }
+
+        if (edev->mutually_exclusive && edev->max_supported) {
+                for (index = 0; edev->mutually_exclusive[index];
+                                index++)
+                        kfree(edev->d_attrs_muex[index].attr.name);
+                kfree(edev->d_attrs_muex);
+                kfree(edev->attrs_muex);
+        }
+
+        for (index = 0; index < edev->max_supported; index++)
+                kfree(edev->cables[index].attr_g.name);
+
+        if (edev->max_supported) {
+                kfree(edev->extcon_dev_type.groups);
+                kfree(edev->cables);
+        }
+
+#if defined(CONFIG_ANDROID)
+        if (switch_class)
+                class_compat_remove_link(switch_class, edev->dev, NULL);
+#endif
+        device_unregister(edev->dev);
+        put_device(edev->dev);
 }
 EXPORT_SYMBOL_GPL(extcon_dev_unregister);
 
@@ -825,6 +842,9 @@ module_init(extcon_class_init);
 
 static void __exit extcon_class_exit(void)
 {
+#if defined(CONFIG_ANDROID)
+        class_compat_unregister(switch_class);
+#endif
         class_destroy(extcon_class);
 }
 module_exit(extcon_class_exit);
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index 3cc152e690b0..71d3ab7b3d8d 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -26,7 +26,6 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/extcon.h>
 #include <linux/workqueue.h>
 #include <linux/gpio.h>
 #include <linux/extcon.h>
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index e21387e2da5c..a17d0d91ada2 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -239,25 +239,19 @@ const char *max77693_extcon_cable[] = {
 static int max77693_muic_set_debounce_time(struct max77693_muic_info *info,
                 enum max77693_muic_adc_debounce_time time)
 {
-        int ret = 0;
-        u8 ctrl3;
+        int ret;
 
         switch (time) {
         case ADC_DEBOUNCE_TIME_5MS:
         case ADC_DEBOUNCE_TIME_10MS:
         case ADC_DEBOUNCE_TIME_25MS:
         case ADC_DEBOUNCE_TIME_38_62MS:
-                ret = max77693_read_reg(info->max77693->regmap_muic,
-                                MAX77693_MUIC_REG_CTRL3, &ctrl3);
-                ctrl3 &= ~CONTROL3_ADCDBSET_MASK;
-                ctrl3 |= (time << CONTROL3_ADCDBSET_SHIFT);
-
-                ret = max77693_write_reg(info->max77693->regmap_muic,
-                                MAX77693_MUIC_REG_CTRL3, ctrl3);
-                if (ret) {
+                ret = max77693_update_reg(info->max77693->regmap_muic,
+                                          MAX77693_MUIC_REG_CTRL3,
+                                          time << CONTROL3_ADCDBSET_SHIFT,
+                                          CONTROL3_ADCDBSET_MASK);
+                if (ret)
                         dev_err(info->dev, "failed to set ADC debounce time\n");
-                        ret = -EINVAL;
-                }
                 break;
         default:
                 dev_err(info->dev, "invalid ADC debounce time\n");
@@ -657,6 +651,8 @@ out:
 static int __devinit max77693_muic_probe(struct platform_device *pdev)
 {
         struct max77693_dev *max77693 = dev_get_drvdata(pdev->dev.parent);
+        struct max77693_platform_data *pdata = dev_get_platdata(max77693->dev);
+        struct max77693_muic_platform_data *muic_pdata = pdata->muic_data;
         struct max77693_muic_info *info;
         int ret, i;
         u8 id;
@@ -727,6 +723,31 @@ static int __devinit max77693_muic_probe(struct platform_device *pdev)
                 goto err_extcon;
         }
 
+        /* Initialize MUIC register by using platform data */
+        for (i = 0 ; i < muic_pdata->num_init_data ; i++) {
+                enum max77693_irq_source irq_src = MAX77693_IRQ_GROUP_NR;
+
+                max77693_write_reg(info->max77693->regmap_muic,
+                                muic_pdata->init_data[i].addr,
+                                muic_pdata->init_data[i].data);
+
+                switch (muic_pdata->init_data[i].addr) {
+                case MAX77693_MUIC_REG_INTMASK1:
+                        irq_src = MUIC_INT1;
+                        break;
+                case MAX77693_MUIC_REG_INTMASK2:
+                        irq_src = MUIC_INT2;
+                        break;
+                case MAX77693_MUIC_REG_INTMASK3:
+                        irq_src = MUIC_INT3;
+                        break;
+                }
+
+                if (irq_src < MAX77693_IRQ_GROUP_NR)
+                        info->max77693->irq_masks_cur[irq_src]
+                                = muic_pdata->init_data[i].data;
+        }
+
         /* Check revision number of MUIC device*/
         ret = max77693_read_reg(info->max77693->regmap_muic,
                         MAX77693_MUIC_REG_ID, &id);
@@ -762,6 +783,7 @@ static int __devexit max77693_muic_remove(struct platform_device *pdev)
                 free_irq(muic_irqs[i].virq, info);
         cancel_work_sync(&info->irq_work);
         extcon_dev_unregister(info->edev);
+        kfree(info->edev);
         kfree(info);
 
         return 0;
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index ef9090a4271d..77b66b0cc8f5 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -271,8 +271,6 @@ out:
 static int max8997_muic_handle_charger_type_detach(
                                 struct max8997_muic_info *info)
 {
-        int ret = 0;
-
         switch (info->pre_charger_type) {
         case MAX8997_CHARGER_TYPE_USB:
                 extcon_set_cable_state(info->edev, "USB", false);
@@ -290,11 +288,11 @@ static int max8997_muic_handle_charger_type_detach(
                 extcon_set_cable_state(info->edev, "Fast-charger", false);
                 break;
         default:
-                ret = -EINVAL;
+                return -EINVAL;
                 break;
         }
 
-        return ret;
+        return 0;
 }
 
 static int max8997_muic_handle_charger_type(struct max8997_muic_info *info,
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 2783f69dada6..f8d22872d753 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -473,8 +473,8 @@ static int ioctl_get_info(struct client *client, union ioctl_arg *arg)
         client->bus_reset_closure = a->bus_reset_closure;
         if (a->bus_reset != 0) {
                 fill_bus_reset_event(&bus_reset, client);
-                ret = copy_to_user(u64_to_uptr(a->bus_reset),
-                                   &bus_reset, sizeof(bus_reset));
+                /* unaligned size of bus_reset is 36 bytes */
+                ret = copy_to_user(u64_to_uptr(a->bus_reset), &bus_reset, 36);
         }
         if (ret == 0 && list_empty(&client->link))
                 list_add_tail(&client->link, &client->device->client_list);
diff --git a/drivers/firmware/memmap.c b/drivers/firmware/memmap.c
index c1cdc9236666..90723e65b081 100644
--- a/drivers/firmware/memmap.c
+++ b/drivers/firmware/memmap.c
@@ -237,7 +237,7 @@ static ssize_t memmap_attr_show(struct kobject *kobj,
  * firmware_map_add() or firmware_map_add_early() afterwards, the entries
  * are not added to sysfs.
  */
-static int __init memmap_init(void)
+static int __init firmware_memmap_init(void)
 {
         struct firmware_map_entry *entry;
 
@@ -246,5 +246,5 @@ static int __init memmap_init(void)
 
         return 0;
 }
-late_initcall(memmap_init);
+late_initcall(firmware_memmap_init);
 
diff --git a/drivers/gpio/gpio-74x164.c b/drivers/gpio/gpio-74x164.c
index ed3e55161bdc..f05e54258ffb 100644
--- a/drivers/gpio/gpio-74x164.c
+++ b/drivers/gpio/gpio-74x164.c
@@ -153,7 +153,7 @@ static int __devinit gen_74x164_probe(struct spi_device *spi)
         }
 
         chip->gpio_chip.ngpio = GEN_74X164_NUMBER_GPIOS * chip->registers;
-        chip->buffer = devm_kzalloc(&spi->dev, chip->gpio_chip.ngpio, GFP_KERNEL);
+        chip->buffer = devm_kzalloc(&spi->dev, chip->registers, GFP_KERNEL);
         if (!chip->buffer) {
                 ret = -ENOMEM;
                 goto exit_destroy;
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index 902af437eaf2..cf7afb9eb61a 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -244,6 +244,8 @@ static int mvebu_gpio_direction_output(struct gpio_chip *chip, unsigned pin,
         if (ret)
                 return ret;
 
+        mvebu_gpio_set(chip, pin, value);
+
         spin_lock_irqsave(&mvchip->lock, flags);
         u = readl_relaxed(mvebu_gpioreg_io_conf(mvchip));
         u &= ~(1 << pin);
@@ -381,11 +383,13 @@ static int mvebu_gpio_irq_set_type(struct irq_data *d, unsigned int type)
                 u = readl_relaxed(mvebu_gpioreg_in_pol(mvchip));
                 u &= ~(1 << pin);
                 writel_relaxed(u, mvebu_gpioreg_in_pol(mvchip));
+                break;
         case IRQ_TYPE_EDGE_FALLING:
         case IRQ_TYPE_LEVEL_LOW:
                 u = readl_relaxed(mvebu_gpioreg_in_pol(mvchip));
                 u |= 1 << pin;
                 writel_relaxed(u, mvebu_gpioreg_in_pol(mvchip));
+                break;
         case IRQ_TYPE_EDGE_BOTH: {
                 u32 v;
 
@@ -401,6 +405,7 @@ static int mvebu_gpio_irq_set_type(struct irq_data *d, unsigned int type)
                 else
                         u &= ~(1 << pin);       /* rising */
                 writel_relaxed(u, mvebu_gpioreg_in_pol(mvchip));
+                break;
         }
         }
         return 0;
@@ -641,7 +646,7 @@ static int __devinit mvebu_gpio_probe(struct platform_device *pdev)
         ct->handler = handle_edge_irq;
         ct->chip.name = mvchip->chip.label;
 
-        irq_setup_generic_chip(gc, IRQ_MSK(ngpios), IRQ_GC_INIT_MASK_CACHE,
+        irq_setup_generic_chip(gc, IRQ_MSK(ngpios), 0,
                                IRQ_NOREQUEST, IRQ_LEVEL | IRQ_NOPROBE);
 
         /* Setup irq domain on top of the generic chip. */
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 94cbc842fbc3..d335af1d4d85 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -251,6 +251,40 @@ static void _set_gpio_debounce(struct gpio_bank *bank, unsigned gpio,
         }
 }
 
+/**
+ * _clear_gpio_debounce - clear debounce settings for a gpio
+ * @bank: the gpio bank we're acting upon
+ * @gpio: the gpio number on this @gpio
+ *
+ * If a gpio is using debounce, then clear the debounce enable bit and if
+ * this is the only gpio in this bank using debounce, then clear the debounce
+ * time too. The debounce clock will also be disabled when calling this function
+ * if this is the only gpio in the bank using debounce.
+ */
+static void _clear_gpio_debounce(struct gpio_bank *bank, unsigned gpio)
+{
+        u32 gpio_bit = GPIO_BIT(bank, gpio);
+
+        if (!bank->dbck_flag)
+                return;
+
+        if (!(bank->dbck_enable_mask & gpio_bit))
+                return;
+
+        bank->dbck_enable_mask &= ~gpio_bit;
+        bank->context.debounce_en &= ~gpio_bit;
+        __raw_writel(bank->context.debounce_en,
+                     bank->base + bank->regs->debounce_en);
+
+        if (!bank->dbck_enable_mask) {
+                bank->context.debounce = 0;
+                __raw_writel(bank->context.debounce, bank->base +
+                             bank->regs->debounce);
+                clk_disable(bank->dbck);
+                bank->dbck_enabled = false;
+        }
+}
+
 static inline void set_gpio_trigger(struct gpio_bank *bank, int gpio,
                                                 unsigned trigger)
 {
@@ -539,6 +573,7 @@ static void _reset_gpio(struct gpio_bank *bank, int gpio)
         _set_gpio_irqenable(bank, gpio, 0);
         _clear_gpio_irqstatus(bank, gpio);
         _set_gpio_triggering(bank, GPIO_INDEX(bank, gpio), IRQ_TYPE_NONE);
+        _clear_gpio_debounce(bank, gpio);
 }
 
 /* Use disable_irq_wake() and enable_irq_wake() functions from drivers */
diff --git a/drivers/gpio/gpio-timberdale.c b/drivers/gpio/gpio-timberdale.c
index 031c6adf5b65..1a3e2b9b4772 100644
--- a/drivers/gpio/gpio-timberdale.c
+++ b/drivers/gpio/gpio-timberdale.c
@@ -116,7 +116,7 @@ static void timbgpio_irq_disable(struct irq_data *d)
         unsigned long flags;
 
         spin_lock_irqsave(&tgpio->lock, flags);
-        tgpio->last_ier &= ~(1 << offset);
+        tgpio->last_ier &= ~(1UL << offset);
         iowrite32(tgpio->last_ier, tgpio->membase + TGPIO_IER);
         spin_unlock_irqrestore(&tgpio->lock, flags);
 }
@@ -128,7 +128,7 @@ static void timbgpio_irq_enable(struct irq_data *d)
         unsigned long flags;
 
         spin_lock_irqsave(&tgpio->lock, flags);
-        tgpio->last_ier |= 1 << offset;
+        tgpio->last_ier |= 1UL << offset;
         iowrite32(tgpio->last_ier, tgpio->membase + TGPIO_IER);
         spin_unlock_irqrestore(&tgpio->lock, flags);
 }
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 5d6c71edc739..1c8d9e3380e1 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -623,9 +623,11 @@ static ssize_t export_store(struct class *class,
          */
 
         status = gpio_request(gpio, "sysfs");
-        if (status < 0)
+        if (status < 0) {
+                if (status == -EPROBE_DEFER)
+                        status = -ENODEV;
                 goto done;
-
+        }
         status = gpio_export(gpio, true);
         if (status < 0)
                 gpio_free(gpio);
@@ -1191,8 +1193,10 @@ int gpio_request(unsigned gpio, const char *label)
 
         spin_lock_irqsave(&gpio_lock, flags);
 
-        if (!gpio_is_valid(gpio))
+        if (!gpio_is_valid(gpio)) {
+                status = -EINVAL;
                 goto done;
+        }
         desc = &gpio_desc[gpio];
         chip = desc->chip;
         if (chip == NULL)
diff --git a/drivers/gpu/drm/drm_fb_cma_helper.c b/drivers/gpu/drm/drm_fb_cma_helper.c
index 09e11a5d921a..fd9d0af4d536 100644
--- a/drivers/gpu/drm/drm_fb_cma_helper.c
+++ b/drivers/gpu/drm/drm_fb_cma_helper.c
@@ -206,7 +206,7 @@ static int drm_fbdev_cma_create(struct drm_fb_helper *helper,
         size_t size;
         int ret;
 
-        DRM_DEBUG_KMS("surface width(%d), height(%d) and bpp(%d\n",
+        DRM_DEBUG_KMS("surface width(%d), height(%d) and bpp(%d)\n",
                         sizes->surface_width, sizes->surface_height,
                         sizes->surface_bpp);
 
@@ -220,7 +220,7 @@ static int drm_fbdev_cma_create(struct drm_fb_helper *helper,
 
         size = mode_cmd.pitches[0] * mode_cmd.height;
         obj = drm_gem_cma_create(dev, size);
-        if (!obj)
+        if (IS_ERR(obj))
                 return -ENOMEM;
 
         fbi = framebuffer_alloc(0, dev->dev);
diff --git a/drivers/gpu/drm/drm_info.c b/drivers/gpu/drm/drm_info.c
index cdf8b1e7602d..d4b20ceda3fb 100644
--- a/drivers/gpu/drm/drm_info.c
+++ b/drivers/gpu/drm/drm_info.c
@@ -205,8 +205,6 @@ static int drm_gem_one_name_info(int id, void *ptr, void *data)
         struct drm_gem_object *obj = ptr;
         struct seq_file *m = data;
 
-        seq_printf(m, "name %d size %zd\n", obj->name, obj->size);
-
         seq_printf(m, "%6d %8zd %7d %8d\n",
                    obj->name, obj->size,
                    atomic_read(&obj->handle_count),
@@ -239,7 +237,7 @@ int drm_vma_info(struct seq_file *m, void *data)
         mutex_lock(&dev->struct_mutex);
         seq_printf(m, "vma use count: %d, high_memory = %pK, 0x%pK\n",
                    atomic_read(&dev->vma_count),
-                   high_memory, (void *)virt_to_phys(high_memory));
+                   high_memory, (void *)(unsigned long)virt_to_phys(high_memory));
 
         list_for_each_entry(pt, &dev->vmalist, head) {
                 vma = pt->vma;
diff --git a/drivers/gpu/drm/drm_platform.c b/drivers/gpu/drm/drm_platform.c
index aaeb6f8d69ce..b8a282ea8751 100644
--- a/drivers/gpu/drm/drm_platform.c
+++ b/drivers/gpu/drm/drm_platform.c
@@ -64,7 +64,6 @@ int drm_get_platform_dev(struct platform_device *platdev,
         }
 
         if (drm_core_check_feature(dev, DRIVER_MODESET)) {
-                dev_set_drvdata(&platdev->dev, dev);
                 ret = drm_get_minor(dev, &dev->control, DRM_MINOR_CONTROL);
                 if (ret)
                         goto err_g1;
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 59a26e577b57..fc345d4ebb03 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -1,6 +1,6 @@
 config DRM_EXYNOS
         tristate "DRM Support for Samsung SoC EXYNOS Series"
-        depends on DRM && PLAT_SAMSUNG
+        depends on DRM && (PLAT_SAMSUNG || ARCH_MULTIPLATFORM)
         select DRM_KMS_HELPER
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.c b/drivers/gpu/drm/exynos/exynos_drm_connector.c
index 18c271862ca8..0f68a2872673 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_connector.c
@@ -374,6 +374,7 @@ struct drm_connector *exynos_drm_connector_create(struct drm_device *dev,
         exynos_connector->encoder_id = encoder->base.id;
         exynos_connector->manager = manager;
         exynos_connector->dpms = DRM_MODE_DPMS_OFF;
+        connector->dpms = DRM_MODE_DPMS_OFF;
         connector->encoder = encoder;
 
         err = drm_mode_connector_attach_encoder(connector, encoder);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.c b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
index e51503fbaf2b..241ad1eeec64 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
@@ -43,12 +43,14 @@
  * @manager: specific encoder has its own manager to control a hardware
  *      appropriately and we can access a hardware drawing on this manager.
  * @dpms: store the encoder dpms value.
+ * @updated: indicate whether overlay data updating is needed or not.
  */
 struct exynos_drm_encoder {
         struct drm_crtc                 *old_crtc;
         struct drm_encoder              drm_encoder;
         struct exynos_drm_manager       *manager;
-        int dpms;
+        int                             dpms;
+        bool                            updated;
 };
 
 static void exynos_drm_connector_power(struct drm_encoder *encoder, int mode)
@@ -85,7 +87,9 @@ static void exynos_drm_encoder_dpms(struct drm_encoder *encoder, int mode)
         switch (mode) {
         case DRM_MODE_DPMS_ON:
                 if (manager_ops && manager_ops->apply)
-                        manager_ops->apply(manager->dev);
+                        if (!exynos_encoder->updated)
+                                manager_ops->apply(manager->dev);
+
                 exynos_drm_connector_power(encoder, mode);
                 exynos_encoder->dpms = mode;
                 break;
@@ -94,6 +98,7 @@ static void exynos_drm_encoder_dpms(struct drm_encoder *encoder, int mode)
         case DRM_MODE_DPMS_OFF:
                 exynos_drm_connector_power(encoder, mode);
                 exynos_encoder->dpms = mode;
+                exynos_encoder->updated = false;
                 break;
         default:
                 DRM_ERROR("unspecified mode %d\n", mode);
@@ -205,13 +210,22 @@ static void exynos_drm_encoder_prepare(struct drm_encoder *encoder)
 
 static void exynos_drm_encoder_commit(struct drm_encoder *encoder)
 {
-        struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
+        struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
+        struct exynos_drm_manager *manager = exynos_encoder->manager;
         struct exynos_drm_manager_ops *manager_ops = manager->ops;
 
         DRM_DEBUG_KMS("%s\n", __FILE__);
 
         if (manager_ops && manager_ops->commit)
                 manager_ops->commit(manager->dev);
+
+        /*
+         * this will avoid one issue that overlay data is updated to
+         * real hardware two times.
+         * And this variable will be used to check if the data was
+         * already updated or not by exynos_drm_encoder_dpms function.
+         */
+        exynos_encoder->updated = true;
 }
 
 static void exynos_drm_encoder_disable(struct drm_encoder *encoder)
@@ -401,19 +415,6 @@ void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data)
                 manager_ops->dpms(manager->dev, mode);
 
         /*
-         * set current mode to new one so that data aren't updated into
-         * registers by drm_helper_connector_dpms two times.
-         *
-         * in case that drm_crtc_helper_set_mode() is called,
-         * overlay_ops->commit() and manager_ops->commit() callbacks
-         * can be called two times, first at drm_crtc_helper_set_mode()
-         * and second at drm_helper_connector_dpms().
-         * so with this setting, when drm_helper_connector_dpms() is called
-         * encoder->funcs->dpms() will be ignored.
-         */
-        exynos_encoder->dpms = mode;
-
-        /*
          * if this condition is ok then it means that the crtc is already
          * detached from encoder and last function for detaching is properly
          * done, so clear pipe from manager to prevent repeated call.
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index a32837951dd2..130a2b510d4a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -19,8 +19,8 @@
 #include <linux/clk.h>
 #include <linux/pm_runtime.h>
 
+#include <video/samsung_fimd.h>
 #include <drm/exynos_drm.h>
-#include <plat/regs-fb-v4.h>
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_fbdev.h"
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index 614b2e9ac462..e7fbb823fd8e 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -1142,7 +1142,7 @@ static int __devinit mixer_probe(struct platform_device *pdev)
                 const struct of_device_id *match;
                 match = of_match_node(of_match_ptr(mixer_match_types),
                                                           pdev->dev.of_node);
-                drv = match->data;
+                drv = (struct mixer_drv_data *)match->data;
         } else {
                 drv = (struct mixer_drv_data *)
                         platform_get_device_id(pdev)->driver_data;
diff --git a/drivers/gpu/drm/i915/dvo_ch7xxx.c b/drivers/gpu/drm/i915/dvo_ch7xxx.c
index 38f3a6cb8c7d..3edd981e0770 100644
--- a/drivers/gpu/drm/i915/dvo_ch7xxx.c
+++ b/drivers/gpu/drm/i915/dvo_ch7xxx.c
@@ -303,10 +303,10 @@ static bool ch7xxx_get_hw_state(struct intel_dvo_device *dvo)
 
         ch7xxx_readb(dvo, CH7xxx_PM, &val);
 
-        if (val & CH7xxx_PM_FPD)
-                return false;
-        else
+        if (val & (CH7xxx_PM_DVIL | CH7xxx_PM_DVIP))
                 return true;
+        else
+                return false;
 }
 
 static void ch7xxx_dump_regs(struct intel_dvo_device *dvo)
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index c9bfd83dde64..61ae104dca8c 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1505,7 +1505,8 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
                 goto put_gmch;
         }
 
-        i915_kick_out_firmware_fb(dev_priv);
+        if (drm_core_check_feature(dev, DRIVER_MODESET))
+                i915_kick_out_firmware_fb(dev_priv);
 
         pci_set_master(dev->pdev);
 
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index aac4e5e1a5b9..6770ee6084b4 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -118,6 +118,13 @@ module_param_named(i915_enable_ppgtt, i915_enable_ppgtt, int, 0600);
 MODULE_PARM_DESC(i915_enable_ppgtt,
                 "Enable PPGTT (default: true)");
 
+unsigned int i915_preliminary_hw_support __read_mostly = 0;
+module_param_named(preliminary_hw_support, i915_preliminary_hw_support, int, 0600);
+MODULE_PARM_DESC(preliminary_hw_support,
+                "Enable preliminary hardware support. "
+                "Enable Haswell and ValleyView Support. "
+                "(default: false)");
+
 static struct drm_driver driver;
 extern int intel_agp_enabled;
 
@@ -826,6 +833,12 @@ i915_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         struct intel_device_info *intel_info =
                 (struct intel_device_info *) ent->driver_data;
 
+        if (intel_info->is_haswell || intel_info->is_valleyview)
+                if(!i915_preliminary_hw_support) {
+                        DRM_ERROR("Preliminary hardware support disabled\n");
+                        return -ENODEV;
+                }
+
         /* Only bind to function 0 of the device. Early generations
          * used function 1 as a placeholder for multi-head. This causes
          * us confusion instead, especially on the systems where both
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 4f2831aa5fed..f511fa2f4168 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -1217,6 +1217,7 @@ extern int i915_enable_rc6 __read_mostly;
 extern int i915_enable_fbc __read_mostly;
 extern bool i915_enable_hangcheck __read_mostly;
 extern int i915_enable_ppgtt __read_mostly;
+extern unsigned int i915_preliminary_hw_support __read_mostly;
 
 extern int i915_suspend(struct drm_device *dev, pm_message_t state);
 extern int i915_resume(struct drm_device *dev);
@@ -1341,9 +1342,14 @@ int __must_check i915_gem_object_get_pages(struct drm_i915_gem_object *obj);
 static inline struct page *i915_gem_object_get_page(struct drm_i915_gem_object *obj, int n)
 {
         struct scatterlist *sg = obj->pages->sgl;
-        while (n >= SG_MAX_SINGLE_ALLOC) {
+        int nents = obj->pages->nents;
+        while (nents > SG_MAX_SINGLE_ALLOC) {
+                if (n < SG_MAX_SINGLE_ALLOC - 1)
+                        break;
+
                 sg = sg_chain_ptr(sg + SG_MAX_SINGLE_ALLOC - 1);
                 n -= SG_MAX_SINGLE_ALLOC - 1;
+                nents -= SG_MAX_SINGLE_ALLOC - 1;
         }
         return sg_page(sg+n);
 }
@@ -1427,7 +1433,7 @@ int __must_check i915_gpu_idle(struct drm_device *dev);
 int __must_check i915_gem_idle(struct drm_device *dev);
 int i915_add_request(struct intel_ring_buffer *ring,
                      struct drm_file *file,
-                     struct drm_i915_gem_request *request);
+                     u32 *seqno);
 int __must_check i915_wait_seqno(struct intel_ring_buffer *ring,
                                  uint32_t seqno);
 int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 19dbdd7dd564..107f09befe92 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -1407,8 +1407,10 @@ out:
                 return VM_FAULT_NOPAGE;
         case -ENOMEM:
                 return VM_FAULT_OOM;
+        case -ENOSPC:
+                return VM_FAULT_SIGBUS;
         default:
-                WARN_ON_ONCE(ret);
+                WARN_ONCE(ret, "unhandled error in i915_gem_fault: %i\n", ret);
                 return VM_FAULT_SIGBUS;
         }
 }
@@ -1822,10 +1824,11 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
                 sg_set_page(sg, page, PAGE_SIZE, 0);
         }
 
+        obj->pages = st;
+
         if (i915_gem_object_needs_bit17_swizzle(obj))
                 i915_gem_object_do_bit_17_swizzle(obj);
 
-        obj->pages = st;
         return 0;
 
 err_pages:
@@ -1955,11 +1958,12 @@ i915_gem_next_request_seqno(struct intel_ring_buffer *ring)
 int
 i915_add_request(struct intel_ring_buffer *ring,
                  struct drm_file *file,
-                 struct drm_i915_gem_request *request)
+                 u32 *out_seqno)
 {
         drm_i915_private_t *dev_priv = ring->dev->dev_private;
-        uint32_t seqno;
+        struct drm_i915_gem_request *request;
         u32 request_ring_position;
+        u32 seqno;
         int was_empty;
         int ret;
 
@@ -1974,11 +1978,9 @@ i915_add_request(struct intel_ring_buffer *ring,
         if (ret)
                 return ret;
 
-        if (request == NULL) {
-                request = kmalloc(sizeof(*request), GFP_KERNEL);
-                if (request == NULL)
-                        return -ENOMEM;
-        }
+        request = kmalloc(sizeof(*request), GFP_KERNEL);
+        if (request == NULL)
+                return -ENOMEM;
 
         seqno = i915_gem_next_request_seqno(ring);
 
@@ -2030,6 +2032,8 @@ i915_add_request(struct intel_ring_buffer *ring,
                 }
         }
 
+        if (out_seqno)
+                *out_seqno = seqno;
         return 0;
 }
 
@@ -3959,6 +3963,9 @@ i915_gem_init_hw(struct drm_device *dev)
         if (!intel_enable_gtt())
                 return -EIO;
 
+        if (IS_HASWELL(dev) && (I915_READ(0x120010) == 1))
+                I915_WRITE(0x9008, I915_READ(0x9008) | 0xf0000);
+
         i915_gem_l3_remap(dev);
 
         i915_gem_init_swizzling(dev);
@@ -4098,7 +4105,6 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
         }
 
         BUG_ON(!list_empty(&dev_priv->mm.active_list));
-        BUG_ON(!list_empty(&dev_priv->mm.inactive_list));
         mutex_unlock(&dev->struct_mutex);
 
         ret = drm_irq_install(dev);
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 64c1be0a9cfd..a4162ddff6c5 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -521,7 +521,7 @@
  */
 # define _3D_CHICKEN2_WM_READ_PIPELINED                 (1 << 14)
 #define _3D_CHICKEN3    0x02090
-#define  _3D_CHICKEN_SF_DISABLE_FASTCLIP_CULL           (1 << 5)
+#define  _3D_CHICKEN3_SF_DISABLE_FASTCLIP_CULL          (1 << 5)
 
 #define MI_MODE         0x0209c
 # define VS_TIMER_DISPATCH                              (1 << 6)
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 893f30164b7e..b726b478a4f5 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -219,20 +219,7 @@ static void intel_crt_mode_set(struct drm_encoder *encoder,
                 intel_encoder_to_crt(to_intel_encoder(encoder));
         struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
         struct drm_i915_private *dev_priv = dev->dev_private;
-        int dpll_md_reg;
-        u32 adpa, dpll_md;
-
-        dpll_md_reg = DPLL_MD(intel_crtc->pipe);
-
-        /*
-         * Disable separate mode multiplier used when cloning SDVO to CRT
-         * XXX this needs to be adjusted when we really are cloning
-         */
-        if (INTEL_INFO(dev)->gen >= 4 && !HAS_PCH_SPLIT(dev)) {
-                dpll_md = I915_READ(dpll_md_reg);
-                I915_WRITE(dpll_md_reg,
-                           dpll_md & ~DPLL_MD_UDI_MULTIPLIER_MASK);
-        }
+        u32 adpa;
 
         adpa = ADPA_HOTPLUG_BITS;
         if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
@@ -742,7 +729,7 @@ void intel_crt_init(struct drm_device *dev)
 
         crt->base.type = INTEL_OUTPUT_ANALOG;
         crt->base.cloneable = true;
-        if (IS_HASWELL(dev))
+        if (IS_HASWELL(dev) || IS_I830(dev))
                 crt->base.crtc_mask = (1 << 0);
         else
                 crt->base.crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 2b6ce9b2674a..461a637f1ef7 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -3253,6 +3253,16 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 
         if (HAS_PCH_CPT(dev))
                 intel_cpt_verify_modeset(dev, intel_crtc->pipe);
+
+        /*
+         * There seems to be a race in PCH platform hw (at least on some
+         * outputs) where an enabled pipe still completes any pageflip right
+         * away (as if the pipe is off) instead of waiting for vblank. As soon
+         * as the first vblank happend, everything works as expected. Hence just
+         * wait for one vblank before returning to avoid strange things
+         * happening.
+         */
+        intel_wait_for_vblank(dev, intel_crtc->pipe);
 }
 
 static void ironlake_crtc_disable(struct drm_crtc *crtc)
@@ -7882,6 +7892,34 @@ struct intel_quirk {
         void (*hook)(struct drm_device *dev);
 };
 
+/* For systems that don't have a meaningful PCI subdevice/subvendor ID */
+struct intel_dmi_quirk {
+        void (*hook)(struct drm_device *dev);
+        const struct dmi_system_id (*dmi_id_list)[];
+};
+
+static int intel_dmi_reverse_brightness(const struct dmi_system_id *id)
+{
+        DRM_INFO("Backlight polarity reversed on %s\n", id->ident);
+        return 1;
+}
+
+static const struct intel_dmi_quirk intel_dmi_quirks[] = {
+        {
+                .dmi_id_list = &(const struct dmi_system_id[]) {
+                        {
+                                .callback = intel_dmi_reverse_brightness,
+                                .ident = "NCR Corporation",
+                                .matches = {DMI_MATCH(DMI_SYS_VENDOR, "NCR Corporation"),
+                                            DMI_MATCH(DMI_PRODUCT_NAME, ""),
+                                },
+                        },
+                        { }  /* terminating entry */
+                },
+                .hook = quirk_invert_brightness,
+        },
+};
+
 static struct intel_quirk intel_quirks[] = {
         /* HP Mini needs pipe A force quirk (LP: #322104) */
         { 0x27ae, 0x103c, 0x361a, quirk_pipea_force },
@@ -7892,8 +7930,7 @@ static struct intel_quirk intel_quirks[] = {
         /* ThinkPad T60 needs pipe A force quirk (bug #16494) */
         { 0x2782, 0x17aa, 0x201a, quirk_pipea_force },
 
-        /* 855 & before need to leave pipe A & dpll A up */
-        { 0x3582, PCI_ANY_ID, PCI_ANY_ID, quirk_pipea_force },
+        /* 830/845 need to leave pipe A & dpll A up */
         { 0x2562, PCI_ANY_ID, PCI_ANY_ID, quirk_pipea_force },
         { 0x3577, PCI_ANY_ID, PCI_ANY_ID, quirk_pipea_force },
 
@@ -7922,6 +7959,10 @@ static void intel_init_quirks(struct drm_device *dev)
                      q->subsystem_device == PCI_ANY_ID))
                         q->hook(dev);
         }
+        for (i = 0; i < ARRAY_SIZE(intel_dmi_quirks); i++) {
+                if (dmi_check_system(*intel_dmi_quirks[i].dmi_id_list) != 0)
+                        intel_dmi_quirks[i].hook(dev);
+        }
 }
 
 /* Disable the VGA plane that we never use */
@@ -8049,29 +8090,42 @@ static void intel_enable_pipe_a(struct drm_device *dev)
 
 }
 
+static bool
+intel_check_plane_mapping(struct intel_crtc *crtc)
+{
+        struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+        u32 reg, val;
+
+        if (dev_priv->num_pipe == 1)
+                return true;
+
+        reg = DSPCNTR(!crtc->plane);
+        val = I915_READ(reg);
+
+        if ((val & DISPLAY_PLANE_ENABLE) &&
+            (!!(val & DISPPLANE_SEL_PIPE_MASK) == crtc->pipe))
+                return false;
+
+        return true;
+}
+
 static void intel_sanitize_crtc(struct intel_crtc *crtc)
 {
         struct drm_device *dev = crtc->base.dev;
         struct drm_i915_private *dev_priv = dev->dev_private;
-        u32 reg, val;
+        u32 reg;
 
         /* Clear any frame start delays used for debugging left by the BIOS */
         reg = PIPECONF(crtc->pipe);
         I915_WRITE(reg, I915_READ(reg) & ~PIPECONF_FRAME_START_DELAY_MASK);
 
         /* We need to sanitize the plane -> pipe mapping first because this will
-         * disable the crtc (and hence change the state) if it is wrong. */
-        if (!HAS_PCH_SPLIT(dev)) {
+         * disable the crtc (and hence change the state) if it is wrong. Note
+         * that gen4+ has a fixed plane -> pipe mapping.  */
+        if (INTEL_INFO(dev)->gen < 4 && !intel_check_plane_mapping(crtc)) {
                 struct intel_connector *connector;
                 bool plane;
 
-                reg = DSPCNTR(crtc->plane);
-                val = I915_READ(reg);
-
-                if ((val & DISPLAY_PLANE_ENABLE) == 0 &&
-                    (!!(val & DISPPLANE_SEL_PIPE_MASK) == crtc->pipe))
-                        goto ok;
-
                 DRM_DEBUG_KMS("[CRTC:%d] wrong plane connection detected!\n",
                               crtc->base.base.id);
 
@@ -8095,7 +8149,6 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
                 WARN_ON(crtc->active);
                 crtc->base.enabled = false;
         }
-ok:
 
         if (dev_priv->quirks & QUIRK_PIPEA_FORCE &&
             crtc->pipe == PIPE_A && !crtc->active) {
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index d1e8ddb2d6c0..368ed8ef1600 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -1797,7 +1797,8 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
                         if ((intel_dp->train_set[i] & DP_TRAIN_MAX_SWING_REACHED) == 0)
                                 break;
                 if (i == intel_dp->lane_count && voltage_tries == 5) {
-                        if (++loop_tries == 5) {
+                        ++loop_tries;
+                        if (loop_tries == 5) {
                                 DRM_DEBUG_KMS("too many full retries, give up\n");
                                 break;
                         }
@@ -1807,11 +1808,15 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
                 }
 
                 /* Check to see if we've tried the same voltage 5 times */
-                if ((intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK) != voltage) {
-                        voltage = intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK;
-                        voltage_tries = 0;
-                } else
+                if ((intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK) == voltage) {
                         ++voltage_tries;
+                        if (voltage_tries == 5) {
+                                DRM_DEBUG_KMS("too many voltage retries, give up\n");
+                                break;
+                        }
+                } else
+                        voltage_tries = 0;
+                voltage = intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK;
 
                 /* Compute new intel_dp->train_set as requested by target */
                 intel_get_adjust_train(intel_dp, link_status);
@@ -2369,8 +2374,9 @@ static void
 intel_dp_destroy(struct drm_connector *connector)
 {
         struct drm_device *dev = connector->dev;
+        struct intel_dp *intel_dp = intel_attached_dp(connector);
 
-        if (intel_dpd_is_edp(dev))
+        if (is_edp(intel_dp))
                 intel_panel_destroy_backlight(dev);
 
         drm_sysfs_connector_remove(connector);
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index e3166df55daa..edba93b3474b 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -777,6 +777,14 @@ static const struct dmi_system_id intel_no_lvds[] = {
                         DMI_MATCH(DMI_BOARD_NAME, "D525TUD"),
                 },
         },
+        {
+                .callback = intel_no_lvds_dmi_callback,
+                .ident = "Supermicro X7SPA-H",
+                .matches = {
+                        DMI_MATCH(DMI_SYS_VENDOR, "Supermicro"),
+                        DMI_MATCH(DMI_PRODUCT_NAME, "X7SPA-H"),
+                },
+        },
 
         { }     /* terminating entry */
 };
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index ebff850a9ab6..d7bc817f51a0 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -209,7 +209,6 @@ static void intel_overlay_unmap_regs(struct intel_overlay *overlay,
 }
 
 static int intel_overlay_do_wait_request(struct intel_overlay *overlay,
-                                         struct drm_i915_gem_request *request,
                                          void (*tail)(struct intel_overlay *))
 {
         struct drm_device *dev = overlay->dev;
@@ -218,12 +217,10 @@ static int intel_overlay_do_wait_request(struct intel_overlay *overlay,
         int ret;
 
         BUG_ON(overlay->last_flip_req);
-        ret = i915_add_request(ring, NULL, request);
-        if (ret) {
-            kfree(request);
-            return ret;
-        }
-        overlay->last_flip_req = request->seqno;
+        ret = i915_add_request(ring, NULL, &overlay->last_flip_req);
+        if (ret)
+                return ret;
+
         overlay->flip_tail = tail;
         ret = i915_wait_seqno(ring, overlay->last_flip_req);
         if (ret)
@@ -240,7 +237,6 @@ static int intel_overlay_on(struct intel_overlay *overlay)
         struct drm_device *dev = overlay->dev;
         struct drm_i915_private *dev_priv = dev->dev_private;
         struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
-        struct drm_i915_gem_request *request;
         int ret;
 
         BUG_ON(overlay->active);
@@ -248,17 +244,9 @@ static int intel_overlay_on(struct intel_overlay *overlay)
 
         WARN_ON(IS_I830(dev) && !(dev_priv->quirks & QUIRK_PIPEA_FORCE));
 
-        request = kzalloc(sizeof(*request), GFP_KERNEL);
-        if (request == NULL) {
-                ret = -ENOMEM;
-                goto out;
-        }
-
         ret = intel_ring_begin(ring, 4);
-        if (ret) {
-                kfree(request);
-                goto out;
-        }
+        if (ret)
+                return ret;
 
         intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_ON);
         intel_ring_emit(ring, overlay->flip_addr | OFC_UPDATE);
@@ -266,9 +254,7 @@ static int intel_overlay_on(struct intel_overlay *overlay)
         intel_ring_emit(ring, MI_NOOP);
         intel_ring_advance(ring);
 
-        ret = intel_overlay_do_wait_request(overlay, request, NULL);
-out:
-        return ret;
+        return intel_overlay_do_wait_request(overlay, NULL);
 }
 
 /* overlay needs to be enabled in OCMD reg */
@@ -278,17 +264,12 @@ static int intel_overlay_continue(struct intel_overlay *overlay,
         struct drm_device *dev = overlay->dev;
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
-        struct drm_i915_gem_request *request;
         u32 flip_addr = overlay->flip_addr;
         u32 tmp;
         int ret;
 
         BUG_ON(!overlay->active);
 
-        request = kzalloc(sizeof(*request), GFP_KERNEL);
-        if (request == NULL)
-                return -ENOMEM;
-
         if (load_polyphase_filter)
                 flip_addr |= OFC_UPDATE;
 
@@ -298,22 +279,14 @@ static int intel_overlay_continue(struct intel_overlay *overlay,
                 DRM_DEBUG("overlay underrun, DOVSTA: %x\n", tmp);
 
         ret = intel_ring_begin(ring, 2);
-        if (ret) {
-                kfree(request);
+        if (ret)
                 return ret;
-        }
+
         intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
         intel_ring_emit(ring, flip_addr);
         intel_ring_advance(ring);
 
-        ret = i915_add_request(ring, NULL, request);
-        if (ret) {
-                kfree(request);
-                return ret;
-        }
-
-        overlay->last_flip_req = request->seqno;
-        return 0;
+        return i915_add_request(ring, NULL, &overlay->last_flip_req);
 }
 
 static void intel_overlay_release_old_vid_tail(struct intel_overlay *overlay)
@@ -349,15 +322,10 @@ static int intel_overlay_off(struct intel_overlay *overlay)
         struct drm_i915_private *dev_priv = dev->dev_private;
         struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
         u32 flip_addr = overlay->flip_addr;
-        struct drm_i915_gem_request *request;
         int ret;
 
         BUG_ON(!overlay->active);
 
-        request = kzalloc(sizeof(*request), GFP_KERNEL);
-        if (request == NULL)
-                return -ENOMEM;
-
         /* According to intel docs the overlay hw may hang (when switching
          * off) without loading the filter coeffs. It is however unclear whether
          * this applies to the disabling of the overlay or to the switching off
@@ -365,22 +333,28 @@ static int intel_overlay_off(struct intel_overlay *overlay)
         flip_addr |= OFC_UPDATE;
 
         ret = intel_ring_begin(ring, 6);
-        if (ret) {
-                kfree(request);
+        if (ret)
                 return ret;
-        }
+
         /* wait for overlay to go idle */
         intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
         intel_ring_emit(ring, flip_addr);
         intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
         /* turn overlay off */
-        intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_OFF);
-        intel_ring_emit(ring, flip_addr);
-        intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+        if (IS_I830(dev)) {
+                /* Workaround: Don't disable the overlay fully, since otherwise
+                 * it dies on the next OVERLAY_ON cmd. */
+                intel_ring_emit(ring, MI_NOOP);
+                intel_ring_emit(ring, MI_NOOP);
+                intel_ring_emit(ring, MI_NOOP);
+        } else {
+                intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_OFF);
+                intel_ring_emit(ring, flip_addr);
+                intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+        }
         intel_ring_advance(ring);
 
-        return intel_overlay_do_wait_request(overlay, request,
-                                             intel_overlay_off_tail);
+        return intel_overlay_do_wait_request(overlay, intel_overlay_off_tail);
 }
 
 /* recover from an interruption due to a signal
@@ -425,24 +399,16 @@ static int intel_overlay_release_old_vid(struct intel_overlay *overlay)
                 return 0;
 
         if (I915_READ(ISR) & I915_OVERLAY_PLANE_FLIP_PENDING_INTERRUPT) {
-                struct drm_i915_gem_request *request;
-
                 /* synchronous slowpath */
-                request = kzalloc(sizeof(*request), GFP_KERNEL);
-                if (request == NULL)
-                        return -ENOMEM;
-
                 ret = intel_ring_begin(ring, 2);
-                if (ret) {
-                        kfree(request);
+                if (ret)
                         return ret;
-                }
 
                 intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
                 intel_ring_emit(ring, MI_NOOP);
                 intel_ring_advance(ring);
 
-                ret = intel_overlay_do_wait_request(overlay, request,
+                ret = intel_overlay_do_wait_request(overlay,
                                                     intel_overlay_release_old_vid_tail);
                 if (ret)
                         return ret;
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index e019b2369861..e2aacd329545 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -435,7 +435,7 @@ int intel_panel_setup_backlight(struct drm_device *dev)
         props.type = BACKLIGHT_RAW;
         props.max_brightness = _intel_panel_get_max_backlight(dev);
         if (props.max_brightness == 0) {
-                DRM_ERROR("Failed to get maximum backlight value\n");
+                DRM_DEBUG_DRIVER("Failed to get maximum backlight value\n");
                 return -ENODEV;
         }
         dev_priv->backlight =
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index b3b4b6cea8b0..72f41aaa71ff 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -3442,8 +3442,8 @@ static void gen6_init_clock_gating(struct drm_device *dev)
                    GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
 
         /* Bspec says we need to always set all mask bits. */
-        I915_WRITE(_3D_CHICKEN, (0xFFFF << 16) |
-                   _3D_CHICKEN_SF_DISABLE_FASTCLIP_CULL);
+        I915_WRITE(_3D_CHICKEN3, (0xFFFF << 16) |
+                   _3D_CHICKEN3_SF_DISABLE_FASTCLIP_CULL);
 
         /*
          * According to the spec the following bits should be
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index 0007a4d9bf6e..79d308da29ff 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -139,6 +139,11 @@ struct intel_sdvo {
 
         /* DDC bus used by this SDVO encoder */
         uint8_t ddc_bus;
+
+        /*
+         * the sdvo flag gets lost in round trip: dtd->adjusted_mode->dtd
+         */
+        uint8_t dtd_sdvo_flags;
 };
 
 struct intel_sdvo_connector {
@@ -889,6 +894,45 @@ static void intel_sdvo_dump_hdmi_buf(struct intel_sdvo *intel_sdvo)
 }
 #endif
 
+static bool intel_sdvo_write_infoframe(struct intel_sdvo *intel_sdvo,
+                                       unsigned if_index, uint8_t tx_rate,
+                                       uint8_t *data, unsigned length)
+{
+        uint8_t set_buf_index[2] = { if_index, 0 };
+        uint8_t hbuf_size, tmp[8];
+        int i;
+
+        if (!intel_sdvo_set_value(intel_sdvo,
+                                  SDVO_CMD_SET_HBUF_INDEX,
+                                  set_buf_index, 2))
+                return false;
+
+        if (!intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_HBUF_INFO,
+                                  &hbuf_size, 1))
+                return false;
+
+        /* Buffer size is 0 based, hooray! */
+        hbuf_size++;
+
+        DRM_DEBUG_KMS("writing sdvo hbuf: %i, hbuf_size %i, hbuf_size: %i\n",
+                      if_index, length, hbuf_size);
+
+        for (i = 0; i < hbuf_size; i += 8) {
+                memset(tmp, 0, 8);
+                if (i < length)
+                        memcpy(tmp, data + i, min_t(unsigned, 8, length - i));
+
+                if (!intel_sdvo_set_value(intel_sdvo,
+                                          SDVO_CMD_SET_HBUF_DATA,
+                                          tmp, 8))
+                        return false;
+        }
+
+        return intel_sdvo_set_value(intel_sdvo,
+                                    SDVO_CMD_SET_HBUF_TXRATE,
+                                    &tx_rate, 1);
+}
+
 static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
 {
         struct dip_infoframe avi_if = {
@@ -896,11 +940,7 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
                 .ver = DIP_VERSION_AVI,
                 .len = DIP_LEN_AVI,
         };
-        uint8_t tx_rate = SDVO_HBUF_TX_VSYNC;
-        uint8_t set_buf_index[2] = { 1, 0 };
         uint8_t sdvo_data[4 + sizeof(avi_if.body.avi)];
-        uint64_t *data = (uint64_t *)sdvo_data;
-        unsigned i;
 
         intel_dip_infoframe_csum(&avi_if);
 
@@ -910,22 +950,9 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
         sdvo_data[3] = avi_if.checksum;
         memcpy(&sdvo_data[4], &avi_if.body, sizeof(avi_if.body.avi));
 
-        if (!intel_sdvo_set_value(intel_sdvo,
-                                  SDVO_CMD_SET_HBUF_INDEX,
-                                  set_buf_index, 2))
-                return false;
-
-        for (i = 0; i < sizeof(sdvo_data); i += 8) {
-                if (!intel_sdvo_set_value(intel_sdvo,
-                                          SDVO_CMD_SET_HBUF_DATA,
-                                          data, 8))
-                        return false;
-                data++;
-        }
-
-        return intel_sdvo_set_value(intel_sdvo,
-                                    SDVO_CMD_SET_HBUF_TXRATE,
-                                    &tx_rate, 1);
+        return intel_sdvo_write_infoframe(intel_sdvo, SDVO_HBUF_INDEX_AVI_IF,
+                                          SDVO_HBUF_TX_VSYNC,
+                                          sdvo_data, sizeof(sdvo_data));
 }
 
 static bool intel_sdvo_set_tv_format(struct intel_sdvo *intel_sdvo)
@@ -984,6 +1011,7 @@ intel_sdvo_get_preferred_input_mode(struct intel_sdvo *intel_sdvo,
                 return false;
 
         intel_sdvo_get_mode_from_dtd(adjusted_mode, &input_dtd);
+        intel_sdvo->dtd_sdvo_flags = input_dtd.part2.sdvo_flags;
 
         return true;
 }
@@ -1092,6 +1120,8 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
          * adjusted_mode.
          */
         intel_sdvo_get_dtd_from_mode(&input_dtd, adjusted_mode);
+        if (intel_sdvo->is_tv || intel_sdvo->is_lvds)
+                input_dtd.part2.sdvo_flags = intel_sdvo->dtd_sdvo_flags;
         if (!intel_sdvo_set_input_timing(intel_sdvo, &input_dtd))
                 DRM_INFO("Setting input timings on %s failed\n",
                          SDVO_NAME(intel_sdvo));
@@ -2277,10 +2307,8 @@ intel_sdvo_lvds_init(struct intel_sdvo *intel_sdvo, int device)
                 intel_sdvo_connector->output_flag = SDVO_OUTPUT_LVDS1;
         }
 
-        /* SDVO LVDS is cloneable because the SDVO encoder does the upscaling,
-         * as opposed to native LVDS, where we upscale with the panel-fitter
-         * (and hence only the native LVDS resolution could be cloned). */
-        intel_sdvo->base.cloneable = true;
+        /* SDVO LVDS is not cloneable because the input mode gets adjusted by the encoder */
+        intel_sdvo->base.cloneable = false;
 
         intel_sdvo_connector_init(intel_sdvo_connector, intel_sdvo);
         if (!intel_sdvo_create_enhance_property(intel_sdvo, intel_sdvo_connector))
diff --git a/drivers/gpu/drm/i915/intel_sdvo_regs.h b/drivers/gpu/drm/i915/intel_sdvo_regs.h
index 9d030142ee43..770bdd6ecd9f 100644
--- a/drivers/gpu/drm/i915/intel_sdvo_regs.h
+++ b/drivers/gpu/drm/i915/intel_sdvo_regs.h
@@ -708,6 +708,8 @@ struct intel_sdvo_enhancements_arg {
 #define SDVO_CMD_SET_AUDIO_STAT         0x91
 #define SDVO_CMD_GET_AUDIO_STAT         0x92
 #define SDVO_CMD_SET_HBUF_INDEX         0x93
+  #define SDVO_HBUF_INDEX_ELD           0
+  #define SDVO_HBUF_INDEX_AVI_IF        1
 #define SDVO_CMD_GET_HBUF_INDEX         0x94
 #define SDVO_CMD_GET_HBUF_INFO          0x95
 #define SDVO_CMD_SET_HBUF_AV_SPLIT      0x96
diff --git a/drivers/gpu/drm/nouveau/core/core/gpuobj.c b/drivers/gpu/drm/nouveau/core/core/gpuobj.c
index 1f34549aff18..70586fde69cf 100644
--- a/drivers/gpu/drm/nouveau/core/core/gpuobj.c
+++ b/drivers/gpu/drm/nouveau/core/core/gpuobj.c
@@ -39,6 +39,11 @@ nouveau_gpuobj_destroy(struct nouveau_gpuobj *gpuobj)
                         nv_wo32(gpuobj, i, 0x00000000);
         }
 
+        if (gpuobj->node) {
+                nouveau_mm_free(&nv_gpuobj(gpuobj->parent)->heap,
+                                &gpuobj->node);
+        }
+
         if (gpuobj->heap.block_size)
                 nouveau_mm_fini(&gpuobj->heap);
 
diff --git a/drivers/gpu/drm/nouveau/core/core/mm.c b/drivers/gpu/drm/nouveau/core/core/mm.c
index bfddf87926dd..a6d3cd6490f7 100644
--- a/drivers/gpu/drm/nouveau/core/core/mm.c
+++ b/drivers/gpu/drm/nouveau/core/core/mm.c
@@ -218,13 +218,16 @@ nouveau_mm_init(struct nouveau_mm *mm, u32 offset, u32 length, u32 block)
         node = kzalloc(sizeof(*node), GFP_KERNEL);
         if (!node)
                 return -ENOMEM;
-        node->offset = roundup(offset, mm->block_size);
-        node->length = rounddown(offset + length, mm->block_size) - node->offset;
+
+        if (length) {
+                node->offset  = roundup(offset, mm->block_size);
+                node->length  = rounddown(offset + length, mm->block_size);
+                node->length -= node->offset;
+        }
 
         list_add_tail(&node->nl_entry, &mm->nodes);
         list_add_tail(&node->fl_entry, &mm->free);
         mm->heap_nodes++;
-        mm->heap_size += length;
         return 0;
 }
 
@@ -236,7 +239,7 @@ nouveau_mm_fini(struct nouveau_mm *mm)
         int nodes = 0;
 
         list_for_each_entry(node, &mm->nodes, nl_entry) {
-                if (nodes++ == mm->heap_nodes)
+                if (WARN_ON(nodes++ == mm->heap_nodes))
                         return -EBUSY;
         }
 
diff --git a/drivers/gpu/drm/nouveau/core/core/parent.c b/drivers/gpu/drm/nouveau/core/core/parent.c
index a1ea034611d5..db7c54943102 100644
--- a/drivers/gpu/drm/nouveau/core/core/parent.c
+++ b/drivers/gpu/drm/nouveau/core/core/parent.c
@@ -101,23 +101,6 @@ nouveau_parent_create_(struct nouveau_object *parent,
         return 0;
 }
 
-int
-_nouveau_parent_ctor(struct nouveau_object *parent,
-                     struct nouveau_object *engine,
-                     struct nouveau_oclass *oclass, void *data, u32 size,
-                     struct nouveau_object **pobject)
-{
-        struct nouveau_parent *object;
-        int ret;
-
-        ret = nouveau_parent_create(parent, engine, oclass, 0, NULL, 0, &object);
-        *pobject = nv_object(object);
-        if (ret)
-                return ret;
-
-        return 0;
-}
-
 void
 nouveau_parent_destroy(struct nouveau_parent *parent)
 {
diff --git a/drivers/gpu/drm/nouveau/core/include/core/mm.h b/drivers/gpu/drm/nouveau/core/include/core/mm.h
index 9ee9bf4028ca..975137ba34a6 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/mm.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/mm.h
@@ -19,7 +19,6 @@ struct nouveau_mm {
 
         u32 block_size;
         int heap_nodes;
-        u32 heap_size;
 };
 
 int  nouveau_mm_init(struct nouveau_mm *, u32 offset, u32 length, u32 block);
diff --git a/drivers/gpu/drm/nouveau/core/include/core/parent.h b/drivers/gpu/drm/nouveau/core/include/core/parent.h
index d3aa251a5eb6..3c2e940eb0f8 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/parent.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/parent.h
@@ -50,9 +50,6 @@ int  nouveau_parent_create_(struct nouveau_object *, struct nouveau_object *,
                             int size, void **);
 void nouveau_parent_destroy(struct nouveau_parent *);
 
-int  _nouveau_parent_ctor(struct nouveau_object *, struct nouveau_object *,
-                          struct nouveau_oclass *, void *, u32,
-                          struct nouveau_object **);
 void _nouveau_parent_dtor(struct nouveau_object *);
 #define _nouveau_parent_init _nouveau_object_init
 #define _nouveau_parent_fini _nouveau_object_fini
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/timer.h b/drivers/gpu/drm/nouveau/core/include/subdev/timer.h
index 49bff901544c..c24ec8ab3db4 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/timer.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/timer.h
@@ -26,7 +26,7 @@ void nouveau_timer_alarm(void *, u32 nsec, struct nouveau_alarm *);
 struct nouveau_timer {
         struct nouveau_subdev base;
         u64  (*read)(struct nouveau_timer *);
-        void (*alarm)(struct nouveau_timer *, u32 time, struct nouveau_alarm *);
+        void (*alarm)(struct nouveau_timer *, u64 time, struct nouveau_alarm *);
 };
 
 static inline struct nouveau_timer *
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
index 2fbb6df697cd..70ca7d5a1aa1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
@@ -72,7 +72,7 @@ nouveau_bios_shadow_of(struct nouveau_bios *bios)
         }
 
         data = of_get_property(dn, "NVDA,BMP", &size);
-        if (data) {
+        if (data && size) {
                 bios->size = size;
                 bios->data = kmalloc(bios->size, GFP_KERNEL);
                 if (bios->data)
@@ -104,6 +104,9 @@ nouveau_bios_shadow_pramin(struct nouveau_bios *bios)
                 goto out;
 
         bios->size = nv_rd08(bios, 0x700002) * 512;
+        if (!bios->size)
+                goto out;
+
         bios->data = kmalloc(bios->size, GFP_KERNEL);
         if (bios->data) {
                 for (i = 0; i < bios->size; i++)
@@ -155,6 +158,9 @@ nouveau_bios_shadow_prom(struct nouveau_bios *bios)
 
         /* read entire bios image to system memory */
         bios->size = nv_rd08(bios, 0x300002) * 512;
+        if (!bios->size)
+                goto out;
+
         bios->data = kmalloc(bios->size, GFP_KERNEL);
         if (bios->data) {
                 for (i = 0; i < bios->size; i++)
@@ -185,23 +191,30 @@ static void
 nouveau_bios_shadow_acpi(struct nouveau_bios *bios)
 {
         struct pci_dev *pdev = nv_device(bios)->pdev;
-        int cnt = 65536 / 4096;
-        int ret;
+        int ret, cnt, i;
 
-        if (!nouveau_acpi_rom_supported(pdev))
+        if (!nouveau_acpi_rom_supported(pdev)) {
+                bios->data = NULL;
                 return;
+        }
 
-        bios->data = kmalloc(65536, GFP_KERNEL);
         bios->size = 0;
-        if (!bios->data)
-                return;
+        bios->data = kmalloc(4096, GFP_KERNEL);
+        if (bios->data) {
+                if (nouveau_acpi_get_bios_chunk(bios->data, 0, 4096) == 4096)
+                        bios->size = bios->data[2] * 512;
+                kfree(bios->data);
+        }
 
-        while (cnt--) {
-                ret = nouveau_acpi_get_bios_chunk(bios->data, bios->size, 4096);
-                if (ret != 4096)
-                        return;
+        if (!bios->size)
+                return;
 
-                bios->size += 4096;
+        bios->data = kmalloc(bios->size, GFP_KERNEL);
+        for (i = 0; bios->data && i < bios->size; i += cnt) {
+                cnt = min((bios->size - i), (u32)4096);
+                ret = nouveau_acpi_get_bios_chunk(bios->data, i, cnt);
+                if (ret != cnt)
+                        break;
         }
 }
 
@@ -230,12 +243,14 @@ nouveau_bios_shadow_pci(struct nouveau_bios *bios)
 static int
 nouveau_bios_score(struct nouveau_bios *bios, const bool writeable)
 {
-        if (!bios->data || bios->data[0] != 0x55 || bios->data[1] != 0xAA) {
+        if (bios->size < 3 || !bios->data || bios->data[0] != 0x55 ||
+                        bios->data[1] != 0xAA) {
                 nv_info(bios, "... signature not found\n");
                 return 0;
         }
 
-        if (nvbios_checksum(bios->data, bios->data[2] * 512)) {
+        if (nvbios_checksum(bios->data,
+                        min_t(u32, bios->data[2] * 512, bios->size))) {
                 nv_info(bios, "... checksum invalid\n");
                 /* if a ro image is somewhat bad, it's probably all rubbish */
                 return writeable ? 2 : 1;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c b/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
index 9ed6e728a94c..7d750382a833 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
@@ -43,7 +43,7 @@ dcb_table(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
         *ver = nv_ro08(bios, dcb);
 
         if (*ver >= 0x41) {
-                nv_warn(bios, "DCB *ver 0x%02x unknown\n", *ver);
+                nv_warn(bios, "DCB version 0x%02x unknown\n", *ver);
                 return 0x0000;
         } else
         if (*ver >= 0x30) {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/pll.c b/drivers/gpu/drm/nouveau/core/subdev/bios/pll.c
index 5e5f4cddae3c..f835501203e5 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/pll.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/pll.c
@@ -157,11 +157,10 @@ pll_map_reg(struct nouveau_bios *bios, u32 reg, u32 *type, u8 *ver, u8 *len)
         while (map->reg) {
                 if (map->reg == reg && *ver >= 0x20) {
                         u16 addr = (data += hdr);
+                        *type = map->type;
                         while (cnt--) {
-                                if (nv_ro32(bios, data) == map->reg) {
-                                        *type = map->type;
+                                if (nv_ro32(bios, data) == map->reg)
                                         return data;
-                                }
                                 data += *len;
                         }
                         return addr;
@@ -200,11 +199,10 @@ pll_map_type(struct nouveau_bios *bios, u8 type, u32 *reg, u8 *ver, u8 *len)
         while (map->reg) {
                 if (map->type == type && *ver >= 0x20) {
                         u16 addr = (data += hdr);
+                        *reg = map->reg;
                         while (cnt--) {
-                                if (nv_ro32(bios, data) == map->reg) {
-                                        *reg = map->reg;
+                                if (nv_ro32(bios, data) == map->reg)
                                         return data;
-                                }
                                 data += *len;
                         }
                         return addr;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c
index fd181fbceddb..f4147f67eda6 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c
@@ -90,6 +90,7 @@ nv50_clock_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
                 return ret;
 
         priv->base.pll_set = nv50_clock_pll_set;
+        priv->base.pll_calc = nv04_clock_pll_calc;
         return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv50.c
index 436e9efe7ef5..5f570806143a 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv50.c
@@ -219,13 +219,11 @@ nv50_fb_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
                              ((priv->base.ram.size & 0x000000ff) << 32);
 
         tags = nv_rd32(priv, 0x100320);
-        if (tags) {
-                ret = nouveau_mm_init(&priv->base.tags, 0, tags, 1);
-                if (ret)
-                        return ret;
+        ret = nouveau_mm_init(&priv->base.tags, 0, tags, 1);
+        if (ret)
+                return ret;
 
-                nv_debug(priv, "%d compression tags\n", tags);
-        }
+        nv_debug(priv, "%d compression tags\n", tags);
 
         size = (priv->base.ram.size >> 12) - rsvd_head - rsvd_tail;
         switch (device->chipset) {
@@ -237,6 +235,7 @@ nv50_fb_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
                         return ret;
 
                 priv->base.ram.stolen = (u64)nv_rd32(priv, 0x100e10) << 12;
+                priv->base.ram.type = NV_MEM_TYPE_STOLEN;
                 break;
         default:
                 ret = nouveau_mm_init(&priv->base.vram, rsvd_head, size,
@@ -277,7 +276,6 @@ nv50_fb_dtor(struct nouveau_object *object)
                 __free_page(priv->r100c08_page);
         }
 
-        nouveau_mm_fini(&priv->base.vram);
         nouveau_fb_destroy(&priv->base);
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
index 3d2c88310f98..dbfc2abf0cfe 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
@@ -292,7 +292,7 @@ nouveau_i2c_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
                 case DCB_I2C_NVIO_BIT:
                         port->drive = info.drive & 0x0f;
                         if (device->card_type < NV_D0) {
-                                if (info.drive >= ARRAY_SIZE(nv50_i2c_port))
+                                if (port->drive >= ARRAY_SIZE(nv50_i2c_port))
                                         break;
                                 port->drive = nv50_i2c_port[port->drive];
                                 port->sense = port->drive;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c b/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c
index b29237970fa0..523178685180 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c
@@ -134,7 +134,7 @@ nouveau_therm_fan_sense(struct nouveau_therm *therm)
         end = ptimer->read(ptimer);
 
         if (cycles == 5) {
-                tach = (u64)60000000000;
+                tach = (u64)60000000000ULL;
                 do_div(tach, (end - start));
                 return tach;
         } else
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
index f87a7a3eb4e7..9360ddd469e7 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
@@ -92,7 +92,7 @@ nv50_fan_pwm_clock(struct nouveau_therm *therm)
                 if (nv_rd32(therm, 0xc040) & 0x800000) {
                         /* Use the HOST clock (100 MHz)
                         * Where does this constant(2.4) comes from? */
-                        pwm_clock = (100000000 >> pwm_div) / 10 / 24;
+                        pwm_clock = (100000000 >> pwm_div) * 10 / 24;
                 } else {
                         /* Where does this constant(20) comes from? */
                         pwm_clock = (crystal * 1000) >> pwm_div;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
index 49976be4d73b..c26ca9bef671 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
@@ -85,7 +85,7 @@ nv04_timer_alarm_trigger(struct nouveau_timer *ptimer)
 }
 
 static void
-nv04_timer_alarm(struct nouveau_timer *ptimer, u32 time,
+nv04_timer_alarm(struct nouveau_timer *ptimer, u64 time,
                  struct nouveau_alarm *alarm)
 {
         struct nv04_timer_priv *priv = (void *)ptimer;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c b/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c
index 0203e1e12caa..49050d991e75 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c
@@ -92,7 +92,8 @@ nv41_vmmgr_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
         struct nv04_vmmgr_priv *priv;
         int ret;
 
-        if (!nouveau_boolopt(device->cfgopt, "NvPCIE", true)) {
+        if (pci_find_capability(device->pdev, PCI_CAP_ID_AGP) ||
+            !nouveau_boolopt(device->cfgopt, "NvPCIE", true)) {
                 return nouveau_object_ctor(parent, engine, &nv04_vmmgr_oclass,
                                            data, size, pobject);
         }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c b/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c
index 0ac18d05a146..aa8131436e3d 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c
@@ -163,7 +163,8 @@ nv44_vmmgr_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
         struct nv04_vmmgr_priv *priv;
         int ret;
 
-        if (!nouveau_boolopt(device->cfgopt, "NvPCIE", true)) {
+        if (pci_find_capability(device->pdev, PCI_CAP_ID_AGP) ||
+            !nouveau_boolopt(device->cfgopt, "NvPCIE", true)) {
                 return nouveau_object_ctor(parent, engine, &nv04_vmmgr_oclass,
                                            data, size, pobject);
         }
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 259e5f1adf47..35ac57f0aab6 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -456,6 +456,7 @@ static struct ttm_tt *
 nouveau_ttm_tt_create(struct ttm_bo_device *bdev, unsigned long size,
                       uint32_t page_flags, struct page *dummy_read)
 {
+#if __OS_HAS_AGP
         struct nouveau_drm *drm = nouveau_bdev(bdev);
         struct drm_device *dev = drm->dev;
 
@@ -463,6 +464,7 @@ nouveau_ttm_tt_create(struct ttm_bo_device *bdev, unsigned long size,
                 return ttm_agp_tt_create(bdev, dev->agp->bridge, size,
                                          page_flags, dummy_read);
         }
+#endif
 
         return nouveau_sgdma_create_ttm(bdev, size, page_flags, dummy_read);
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 8f98e5a8c488..86124b131f4f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -290,6 +290,7 @@ nouveau_display_create(struct drm_device *dev)
         struct nouveau_drm *drm = nouveau_drm(dev);
         struct nouveau_disp *pdisp = nouveau_disp(drm->device);
         struct nouveau_display *disp;
+        u32 pclass = dev->pdev->class >> 8;
         int ret, gen;
 
         disp = drm->display = kzalloc(sizeof(*disp), GFP_KERNEL);
@@ -360,23 +361,27 @@ nouveau_display_create(struct drm_device *dev)
         drm_kms_helper_poll_init(dev);
         drm_kms_helper_poll_disable(dev);
 
-        if (nv_device(drm->device)->card_type < NV_50)
-                ret = nv04_display_create(dev);
-        else
-        if (nv_device(drm->device)->card_type < NV_D0)
-                ret = nv50_display_create(dev);
-        else
-                ret = nvd0_display_create(dev);
-        if (ret)
-                goto disp_create_err;
-
-        if (dev->mode_config.num_crtc) {
-                ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
+        if (nouveau_modeset == 1 ||
+            (nouveau_modeset < 0 && pclass == PCI_CLASS_DISPLAY_VGA)) {
+                if (nv_device(drm->device)->card_type < NV_50)
+                        ret = nv04_display_create(dev);
+                else
+                if (nv_device(drm->device)->card_type < NV_D0)
+                        ret = nv50_display_create(dev);
+                else
+                        ret = nvd0_display_create(dev);
                 if (ret)
-                        goto vblank_err;
+                        goto disp_create_err;
+
+                if (dev->mode_config.num_crtc) {
+                        ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
+                        if (ret)
+                                goto vblank_err;
+                }
+
+                nouveau_backlight_init(dev);
         }
 
-        nouveau_backlight_init(dev);
         return 0;
 
 vblank_err:
@@ -395,7 +400,8 @@ nouveau_display_destroy(struct drm_device *dev)
         nouveau_backlight_exit(dev);
         drm_vblank_cleanup(dev);
 
-        disp->dtor(dev);
+        if (disp->dtor)
+                disp->dtor(dev);
 
         drm_kms_helper_poll_fini(dev);
         drm_mode_config_cleanup(dev);
@@ -530,9 +536,11 @@ nouveau_page_flip_reserve(struct nouveau_bo *old_bo,
         if (ret)
                 goto fail;
 
-        ret = ttm_bo_reserve(&old_bo->bo, false, false, false, 0);
-        if (ret)
-                goto fail_unreserve;
+        if (likely(old_bo != new_bo)) {
+                ret = ttm_bo_reserve(&old_bo->bo, false, false, false, 0);
+                if (ret)
+                        goto fail_unreserve;
+        }
 
         return 0;
 
@@ -551,8 +559,10 @@ nouveau_page_flip_unreserve(struct nouveau_bo *old_bo,
         nouveau_bo_fence(new_bo, fence);
         ttm_bo_unreserve(&new_bo->bo);
 
-        nouveau_bo_fence(old_bo, fence);
-        ttm_bo_unreserve(&old_bo->bo);
+        if (likely(old_bo != new_bo)) {
+                nouveau_bo_fence(old_bo, fence);
+                ttm_bo_unreserve(&old_bo->bo);
+        }
 
         nouveau_bo_unpin(old_bo);
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index ccae8c26ae2b..0910125cbbc3 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -63,8 +63,9 @@ MODULE_PARM_DESC(noaccel, "disable kernel/abi16 acceleration");
 static int nouveau_noaccel = 0;
 module_param_named(noaccel, nouveau_noaccel, int, 0400);
 
-MODULE_PARM_DESC(modeset, "enable driver");
-static int nouveau_modeset = -1;
+MODULE_PARM_DESC(modeset, "enable driver (default: auto, "
+                          "0 = disabled, 1 = enabled, 2 = headless)");
+int nouveau_modeset = -1;
 module_param_named(modeset, nouveau_modeset, int, 0400);
 
 static struct drm_driver driver;
@@ -363,7 +364,8 @@ nouveau_drm_unload(struct drm_device *dev)
 
         nouveau_pm_fini(dev);
 
-        nouveau_display_fini(dev);
+        if (dev->mode_config.num_crtc)
+                nouveau_display_fini(dev);
         nouveau_display_destroy(dev);
 
         nouveau_irq_fini(dev);
@@ -403,13 +405,15 @@ nouveau_drm_suspend(struct pci_dev *pdev, pm_message_t pm_state)
             pm_state.event == PM_EVENT_PRETHAW)
                 return 0;
 
-        NV_INFO(drm, "suspending fbcon...\n");
-        nouveau_fbcon_set_suspend(dev, 1);
+        if (dev->mode_config.num_crtc) {
+                NV_INFO(drm, "suspending fbcon...\n");
+                nouveau_fbcon_set_suspend(dev, 1);
 
-        NV_INFO(drm, "suspending display...\n");
-        ret = nouveau_display_suspend(dev);
-        if (ret)
-                return ret;
+                NV_INFO(drm, "suspending display...\n");
+                ret = nouveau_display_suspend(dev);
+                if (ret)
+                        return ret;
+        }
 
         NV_INFO(drm, "evicting buffers...\n");
         ttm_bo_evict_mm(&drm->ttm.bdev, TTM_PL_VRAM);
@@ -445,8 +449,10 @@ fail_client:
                 nouveau_client_init(&cli->base);
         }
 
-        NV_INFO(drm, "resuming display...\n");
-        nouveau_display_resume(dev);
+        if (dev->mode_config.num_crtc) {
+                NV_INFO(drm, "resuming display...\n");
+                nouveau_display_resume(dev);
+        }
         return ret;
 }
 
@@ -486,8 +492,10 @@ nouveau_drm_resume(struct pci_dev *pdev)
         nouveau_irq_postinstall(dev);
         nouveau_pm_resume(dev);
 
-        NV_INFO(drm, "resuming display...\n");
-        nouveau_display_resume(dev);
+        if (dev->mode_config.num_crtc) {
+                NV_INFO(drm, "resuming display...\n");
+                nouveau_display_resume(dev);
+        }
         return 0;
 }
 
@@ -662,9 +670,7 @@ nouveau_drm_init(void)
 #ifdef CONFIG_VGA_CONSOLE
                 if (vgacon_text_force())
                         nouveau_modeset = 0;
-                else
 #endif
-                        nouveau_modeset = 1;
         }
 
         if (!nouveau_modeset)
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.h b/drivers/gpu/drm/nouveau/nouveau_drm.h
index 819471217546..a10169927086 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.h
@@ -141,4 +141,6 @@ int nouveau_drm_resume(struct pci_dev *);
                 nv_info((cli), fmt, ##args);                                   \
 } while (0)
 
+extern int nouveau_modeset;
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_irq.c b/drivers/gpu/drm/nouveau/nouveau_irq.c
index 9ca8afdb5549..1d8cb506a28a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_irq.c
+++ b/drivers/gpu/drm/nouveau/nouveau_irq.c
@@ -61,13 +61,15 @@ nouveau_irq_handler(DRM_IRQ_ARGS)
 
         nv_subdev(pmc)->intr(nv_subdev(pmc));
 
-        if (device->card_type >= NV_D0) {
-                if (nv_rd32(device, 0x000100) & 0x04000000)
-                        nvd0_display_intr(dev);
-        } else
-        if (device->card_type >= NV_50) {
-                if (nv_rd32(device, 0x000100) & 0x04000000)
-                        nv50_display_intr(dev);
+        if (dev->mode_config.num_crtc) {
+                if (device->card_type >= NV_D0) {
+                        if (nv_rd32(device, 0x000100) & 0x04000000)
+                                nvd0_display_intr(dev);
+                } else
+                if (device->card_type >= NV_50) {
+                        if (nv_rd32(device, 0x000100) & 0x04000000)
+                                nv50_display_intr(dev);
+                }
         }
 
         return IRQ_HANDLED;
diff --git a/drivers/gpu/drm/nouveau/nouveau_pm.c b/drivers/gpu/drm/nouveau/nouveau_pm.c
index 0bf64c90aa20..5566172774df 100644
--- a/drivers/gpu/drm/nouveau/nouveau_pm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_pm.c
@@ -52,7 +52,7 @@ nouveau_pm_perflvl_aux(struct drm_device *dev, struct nouveau_pm_level *perflvl,
 {
         struct nouveau_drm *drm = nouveau_drm(dev);
         struct nouveau_pm *pm = nouveau_pm(dev);
-        struct nouveau_therm *therm = nouveau_therm(drm);
+        struct nouveau_therm *therm = nouveau_therm(drm->device);
         int ret;
 
         /*XXX: not on all boards, we should control based on temperature
@@ -64,7 +64,6 @@ nouveau_pm_perflvl_aux(struct drm_device *dev, struct nouveau_pm_level *perflvl,
                 ret = therm->fan_set(therm, perflvl->fanspeed);
                 if (ret && ret != -ENODEV) {
                         NV_ERROR(drm, "fanspeed set failed: %d\n", ret);
-                        return ret;
                 }
         }
 
@@ -706,8 +705,7 @@ nouveau_hwmon_init(struct drm_device *dev)
         struct device *hwmon_dev;
         int ret = 0;
 
-        if (!therm || !therm->temp_get || !therm->attr_get ||
-                !therm->attr_set || therm->temp_get(therm) < 0)
+        if (!therm || !therm->temp_get || !therm->attr_get || !therm->attr_set)
                 return -ENODEV;
 
         hwmon_dev = hwmon_device_register(&dev->pdev->dev);
diff --git a/drivers/gpu/drm/nouveau/nv04_dac.c b/drivers/gpu/drm/nouveau/nv04_dac.c
index 347a3bd78d04..64f7020fb605 100644
--- a/drivers/gpu/drm/nouveau/nv04_dac.c
+++ b/drivers/gpu/drm/nouveau/nv04_dac.c
@@ -220,7 +220,7 @@ out:
         NVWriteVgaCrtc(dev, 0, NV_CIO_CR_MODE_INDEX, saved_cr_mode);
 
         if (blue == 0x18) {
-                NV_INFO(drm, "Load detected on head A\n");
+                NV_DEBUG(drm, "Load detected on head A\n");
                 return connector_status_connected;
         }
 
@@ -338,8 +338,8 @@ nv17_dac_detect(struct drm_encoder *encoder, struct drm_connector *connector)
 
         if (nv17_dac_sample_load(encoder) &
             NV_PRAMDAC_TEST_CONTROL_SENSEB_ALLHI) {
-                NV_INFO(drm, "Load detected on output %c\n",
-                        '@' + ffs(dcb->or));
+                NV_DEBUG(drm, "Load detected on output %c\n",
+                         '@' + ffs(dcb->or));
                 return connector_status_connected;
         } else {
                 return connector_status_disconnected;
@@ -413,9 +413,9 @@ static void nv04_dac_commit(struct drm_encoder *encoder)
 
         helper->dpms(encoder, DRM_MODE_DPMS_ON);
 
-        NV_INFO(drm, "Output %s is running on CRTC %d using output %c\n",
-                drm_get_connector_name(&nouveau_encoder_connector_get(nv_encoder)->base),
-                nv_crtc->index, '@' + ffs(nv_encoder->dcb->or));
+        NV_DEBUG(drm, "Output %s is running on CRTC %d using output %c\n",
+                 drm_get_connector_name(&nouveau_encoder_connector_get(nv_encoder)->base),
+                 nv_crtc->index, '@' + ffs(nv_encoder->dcb->or));
 }
 
 void nv04_dac_update_dacclk(struct drm_encoder *encoder, bool enable)
@@ -461,8 +461,8 @@ static void nv04_dac_dpms(struct drm_encoder *encoder, int mode)
                 return;
         nv_encoder->last_dpms = mode;
 
-        NV_INFO(drm, "Setting dpms mode %d on vga encoder (output %d)\n",
-                     mode, nv_encoder->dcb->index);
+        NV_DEBUG(drm, "Setting dpms mode %d on vga encoder (output %d)\n",
+                 mode, nv_encoder->dcb->index);
 
         nv04_dac_update_dacclk(encoder, mode == DRM_MODE_DPMS_ON);
 }
diff --git a/drivers/gpu/drm/nouveau/nv04_dfp.c b/drivers/gpu/drm/nouveau/nv04_dfp.c
index da55d7642c8c..184cdf806761 100644
--- a/drivers/gpu/drm/nouveau/nv04_dfp.c
+++ b/drivers/gpu/drm/nouveau/nv04_dfp.c
@@ -476,9 +476,9 @@ static void nv04_dfp_commit(struct drm_encoder *encoder)
 
         helper->dpms(encoder, DRM_MODE_DPMS_ON);
 
-        NV_INFO(drm, "Output %s is running on CRTC %d using output %c\n",
-                drm_get_connector_name(&nouveau_encoder_connector_get(nv_encoder)->base),
-                nv_crtc->index, '@' + ffs(nv_encoder->dcb->or));
+        NV_DEBUG(drm, "Output %s is running on CRTC %d using output %c\n",
+                 drm_get_connector_name(&nouveau_encoder_connector_get(nv_encoder)->base),
+                 nv_crtc->index, '@' + ffs(nv_encoder->dcb->or));
 }
 
 static void nv04_dfp_update_backlight(struct drm_encoder *encoder, int mode)
@@ -520,8 +520,8 @@ static void nv04_lvds_dpms(struct drm_encoder *encoder, int mode)
                 return;
         nv_encoder->last_dpms = mode;
 
-        NV_INFO(drm, "Setting dpms mode %d on lvds encoder (output %d)\n",
-                     mode, nv_encoder->dcb->index);
+        NV_DEBUG(drm, "Setting dpms mode %d on lvds encoder (output %d)\n",
+                 mode, nv_encoder->dcb->index);
 
         if (was_powersaving && is_powersaving_dpms(mode))
                 return;
@@ -565,8 +565,8 @@ static void nv04_tmds_dpms(struct drm_encoder *encoder, int mode)
                 return;
         nv_encoder->last_dpms = mode;
 
-        NV_INFO(drm, "Setting dpms mode %d on tmds encoder (output %d)\n",
-                     mode, nv_encoder->dcb->index);
+        NV_DEBUG(drm, "Setting dpms mode %d on tmds encoder (output %d)\n",
+                 mode, nv_encoder->dcb->index);
 
         nv04_dfp_update_backlight(encoder, mode);
         nv04_dfp_update_fp_control(encoder, mode);
diff --git a/drivers/gpu/drm/nouveau/nv04_tv.c b/drivers/gpu/drm/nouveau/nv04_tv.c
index 099fbeda6e2e..62e826a139b3 100644
--- a/drivers/gpu/drm/nouveau/nv04_tv.c
+++ b/drivers/gpu/drm/nouveau/nv04_tv.c
@@ -75,8 +75,8 @@ static void nv04_tv_dpms(struct drm_encoder *encoder, int mode)
         struct nv04_mode_state *state = &nv04_display(dev)->mode_reg;
         uint8_t crtc1A;
 
-        NV_INFO(drm, "Setting dpms mode %d on TV encoder (output %d)\n",
-                mode, nv_encoder->dcb->index);
+        NV_DEBUG(drm, "Setting dpms mode %d on TV encoder (output %d)\n",
+                 mode, nv_encoder->dcb->index);
 
         state->pllsel &= ~(PLLSEL_TV_CRTC1_MASK | PLLSEL_TV_CRTC2_MASK);
 
@@ -167,9 +167,8 @@ static void nv04_tv_commit(struct drm_encoder *encoder)
 
         helper->dpms(encoder, DRM_MODE_DPMS_ON);
 
-        NV_INFO(drm, "Output %s is running on CRTC %d using output %c\n",
-                      drm_get_connector_name(&nouveau_encoder_connector_get(nv_encoder)->base), nv_crtc->index,
-                      '@' + ffs(nv_encoder->dcb->or));
+        NV_DEBUG(drm, "Output %s is running on CRTC %d using output %c\n",
+                 drm_get_connector_name(&nouveau_encoder_connector_get(nv_encoder)->base), nv_crtc->index, '@' + ffs(nv_encoder->dcb->or));
 }
 
 static void nv04_tv_destroy(struct drm_encoder *encoder)
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index 96184d02c8d9..2e566e123e9e 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -1690,10 +1690,10 @@ static int radeon_atom_pick_pll(struct drm_crtc *crtc)
                 }
                 /* all other cases */
                 pll_in_use = radeon_get_pll_use_mask(crtc);
-                if (!(pll_in_use & (1 << ATOM_PPLL2)))
-                        return ATOM_PPLL2;
                 if (!(pll_in_use & (1 << ATOM_PPLL1)))
                         return ATOM_PPLL1;
+                if (!(pll_in_use & (1 << ATOM_PPLL2)))
+                        return ATOM_PPLL2;
                 DRM_ERROR("unable to allocate a PPLL\n");
                 return ATOM_PPLL_INVALID;
         } else {
@@ -1715,10 +1715,10 @@ static int radeon_atom_pick_pll(struct drm_crtc *crtc)
                         }
                         /* all other cases */
                         pll_in_use = radeon_get_pll_use_mask(crtc);
-                        if (!(pll_in_use & (1 << ATOM_PPLL2)))
-                                return ATOM_PPLL2;
                         if (!(pll_in_use & (1 << ATOM_PPLL1)))
                                 return ATOM_PPLL1;
+                        if (!(pll_in_use & (1 << ATOM_PPLL2)))
+                                return ATOM_PPLL2;
                         DRM_ERROR("unable to allocate a PPLL\n");
                         return ATOM_PPLL_INVALID;
                 } else {
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 49cbb3795a10..ba498f8e47a2 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -184,6 +184,7 @@ void radeon_atom_backlight_init(struct radeon_encoder *radeon_encoder,
         struct radeon_backlight_privdata *pdata;
         struct radeon_encoder_atom_dig *dig;
         u8 backlight_level;
+        char bl_name[16];
 
         if (!radeon_encoder->enc_priv)
                 return;
@@ -203,7 +204,9 @@ void radeon_atom_backlight_init(struct radeon_encoder *radeon_encoder,
         memset(&props, 0, sizeof(props));
         props.max_brightness = RADEON_MAX_BL_LEVEL;
         props.type = BACKLIGHT_RAW;
-        bd = backlight_device_register("radeon_bl", &drm_connector->kdev,
+        snprintf(bl_name, sizeof(bl_name),
+                 "radeon_bl%d", dev->primary->index);
+        bd = backlight_device_register(bl_name, &drm_connector->kdev,
                                        pdata, &radeon_atom_backlight_ops, &props);
         if (IS_ERR(bd)) {
                 DRM_ERROR("Backlight registration failed\n");
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index a1f49c5fd74b..14313ad43b76 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -3431,9 +3431,14 @@ void evergreen_pcie_gen2_enable(struct radeon_device *rdev)
         if (!(mask & DRM_PCIE_SPEED_50))
                 return;
 
+        speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+        if (speed_cntl & LC_CURRENT_DATA_RATE) {
+                DRM_INFO("PCIE gen 2 link speeds already enabled\n");
+                return;
+        }
+
         DRM_INFO("enabling PCIE gen 2 link speeds, disable with radeon.pcie_gen2=0\n");
 
-        speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
         if ((speed_cntl & LC_OTHER_SIDE_EVER_SENT_GEN2) ||
             (speed_cntl & LC_OTHER_SIDE_SUPPORTS_GEN2)) {
 
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
index 573ed1bc6cf7..95e6318b6268 100644
--- a/drivers/gpu/drm/radeon/evergreen_cs.c
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -264,7 +264,7 @@ static int evergreen_surface_check_2d(struct radeon_cs_parser *p,
         /* macro tile width & height */
         palign = (8 * surf->bankw * track->npipes) * surf->mtilea;
         halign = (8 * surf->bankh * surf->nbanks) / surf->mtilea;
-        mtileb = (palign / 8) * (halign / 8) * tileb;;
+        mtileb = (palign / 8) * (halign / 8) * tileb;
         mtile_pr = surf->nbx / palign;
         mtile_ps = (mtile_pr * surf->nby) / halign;
         surf->layer_size = mtile_ps * mtileb * slice_pt;
@@ -2829,6 +2829,7 @@ static bool evergreen_vm_reg_valid(u32 reg)
         case CAYMAN_SQ_EX_ALLOC_TABLE_SLOTS:
                 return true;
         default:
+                DRM_ERROR("Invalid register 0x%x in CS\n", reg);
                 return false;
         }
 }
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 8bcb554ea0c5..81e6a568c29d 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -770,9 +770,13 @@ static int cayman_pcie_gart_enable(struct radeon_device *rdev)
         WREG32(0x15DC, 0);
 
         /* empty context1-7 */
+        /* Assign the pt base to something valid for now; the pts used for
+         * the VMs are determined by the application and setup and assigned
+         * on the fly in the vm part of radeon_gart.c
+         */
         for (i = 1; i < 8; i++) {
                 WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR + (i << 2), 0);
-                WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (i << 2), 0);
+                WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (i << 2), rdev->vm_manager.max_pfn);
                 WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2),
                         rdev->gart.table_addr >> 12);
         }
@@ -1534,26 +1538,31 @@ void cayman_vm_set_page(struct radeon_device *rdev, uint64_t pe,
 {
         struct radeon_ring *ring = &rdev->ring[rdev->asic->vm.pt_ring_index];
         uint32_t r600_flags = cayman_vm_page_flags(rdev, flags);
-        int i;
 
-        radeon_ring_write(ring, PACKET3(PACKET3_ME_WRITE, 1 + count * 2));
-        radeon_ring_write(ring, pe);
-        radeon_ring_write(ring, upper_32_bits(pe) & 0xff);
-        for (i = 0; i < count; ++i) {
-                uint64_t value = 0;
-                if (flags & RADEON_VM_PAGE_SYSTEM) {
-                        value = radeon_vm_map_gart(rdev, addr);
-                        value &= 0xFFFFFFFFFFFFF000ULL;
-                        addr += incr;
-
-                } else if (flags & RADEON_VM_PAGE_VALID) {
-                        value = addr;
-                        addr += incr;
-                }
+        while (count) {
+                unsigned ndw = 1 + count * 2;
+                if (ndw > 0x3FFF)
+                        ndw = 0x3FFF;
+
+                radeon_ring_write(ring, PACKET3(PACKET3_ME_WRITE, ndw));
+                radeon_ring_write(ring, pe);
+                radeon_ring_write(ring, upper_32_bits(pe) & 0xff);
+                for (; ndw > 1; ndw -= 2, --count, pe += 8) {
+                        uint64_t value = 0;
+                        if (flags & RADEON_VM_PAGE_SYSTEM) {
+                                value = radeon_vm_map_gart(rdev, addr);
+                                value &= 0xFFFFFFFFFFFFF000ULL;
+                                addr += incr;
+
+                        } else if (flags & RADEON_VM_PAGE_VALID) {
+                                value = addr;
+                                addr += incr;
+                        }
 
-                value |= r600_flags;
-                radeon_ring_write(ring, value);
-                radeon_ring_write(ring, upper_32_bits(value));
+                        value |= r600_flags;
+                        radeon_ring_write(ring, value);
+                        radeon_ring_write(ring, upper_32_bits(value));
+                }
         }
 }
 
@@ -1572,12 +1581,6 @@ void cayman_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
         if (vm == NULL)
                 return;
 
-        radeon_ring_write(ring, PACKET0(VM_CONTEXT0_PAGE_TABLE_START_ADDR + (vm->id << 2), 0));
-        radeon_ring_write(ring, 0);
-
-        radeon_ring_write(ring, PACKET0(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (vm->id << 2), 0));
-        radeon_ring_write(ring, vm->last_pfn);
-
         radeon_ring_write(ring, PACKET0(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2), 0));
         radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
 
@@ -1588,4 +1591,8 @@ void cayman_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
         /* bits 0-7 are the VM contexts0-7 */
         radeon_ring_write(ring, PACKET0(VM_INVALIDATE_REQUEST, 0));
         radeon_ring_write(ring, 1 << vm->id);
+
+        /* sync PFP to ME, otherwise we might get invalid PFP reads */
+        radeon_ring_write(ring, PACKET3(PACKET3_PFP_SYNC_ME, 0));
+        radeon_ring_write(ring, 0x0);
 }
diff --git a/drivers/gpu/drm/radeon/nid.h b/drivers/gpu/drm/radeon/nid.h
index 2423d1b5d385..cbef6815907a 100644
--- a/drivers/gpu/drm/radeon/nid.h
+++ b/drivers/gpu/drm/radeon/nid.h
@@ -502,6 +502,7 @@
 #define PACKET3_MPEG_INDEX                              0x3A
 #define PACKET3_WAIT_REG_MEM                            0x3C
 #define PACKET3_MEM_WRITE                               0x3D
+#define PACKET3_PFP_SYNC_ME                             0x42
 #define PACKET3_SURFACE_SYNC                            0x43
 #              define PACKET3_CB0_DEST_BASE_ENA    (1 << 6)
 #              define PACKET3_CB1_DEST_BASE_ENA    (1 << 7)
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 70c800ff6190..cda280d157da 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -3703,6 +3703,12 @@ static void r600_pcie_gen2_enable(struct radeon_device *rdev)
         if (!(mask & DRM_PCIE_SPEED_50))
                 return;
 
+        speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+        if (speed_cntl & LC_CURRENT_DATA_RATE) {
+                DRM_INFO("PCIE gen 2 link speeds already enabled\n");
+                return;
+        }
+
         DRM_INFO("enabling PCIE gen 2 link speeds, disable with radeon.pcie_gen2=0\n");
 
         /* 55 nm r6xx asics */
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index b04c06444d8b..8c42d54c2e26 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -663,9 +663,14 @@ struct radeon_vm {
         struct list_head                list;
         struct list_head                va;
         unsigned                        id;
-        unsigned                        last_pfn;
-        u64                             pd_gpu_addr;
-        struct radeon_sa_bo             *sa_bo;
+
+        /* contains the page directory */
+        struct radeon_sa_bo             *page_directory;
+        uint64_t                        pd_gpu_addr;
+
+        /* array of page tables, one for each page directory entry */
+        struct radeon_sa_bo             **page_tables;
+
         struct mutex                    mutex;
         /* last fence for cs using this vm */
         struct radeon_fence             *fence;
@@ -1843,9 +1848,10 @@ extern void radeon_ttm_set_active_vram_size(struct radeon_device *rdev, u64 size
  */
 int radeon_vm_manager_init(struct radeon_device *rdev);
 void radeon_vm_manager_fini(struct radeon_device *rdev);
-int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm);
+void radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm);
 void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm);
 int radeon_vm_alloc_pt(struct radeon_device *rdev, struct radeon_vm *vm);
+void radeon_vm_add_to_lru(struct radeon_device *rdev, struct radeon_vm *vm);
 struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
                                        struct radeon_vm *vm, int ring);
 void radeon_vm_fence(struct radeon_device *rdev,
diff --git a/drivers/gpu/drm/radeon/radeon_acpi.c b/drivers/gpu/drm/radeon/radeon_acpi.c
index b0a5688c67f8..196d28d99570 100644
--- a/drivers/gpu/drm/radeon/radeon_acpi.c
+++ b/drivers/gpu/drm/radeon/radeon_acpi.c
@@ -201,7 +201,7 @@ static int radeon_atif_verify_interface(acpi_handle handle,
 
         size = *(u16 *) info->buffer.pointer;
         if (size < 12) {
-                DRM_INFO("ATIF buffer is too small: %lu\n", size);
+                DRM_INFO("ATIF buffer is too small: %zu\n", size);
                 err = -EINVAL;
                 goto out;
         }
@@ -370,6 +370,7 @@ int radeon_atif_handler(struct radeon_device *rdev,
 
                         radeon_set_backlight_level(rdev, enc, req.backlight_level);
 
+#if defined(CONFIG_BACKLIGHT_CLASS_DEVICE) || defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE)
                         if (rdev->is_atom_bios) {
                                 struct radeon_encoder_atom_dig *dig = enc->enc_priv;
                                 backlight_force_update(dig->bl_dev,
@@ -379,6 +380,7 @@ int radeon_atif_handler(struct radeon_device *rdev,
                                 backlight_force_update(dig->bl_dev,
                                                        BACKLIGHT_UPDATE_HOTKEY);
                         }
+#endif
                 }
         }
         /* TODO: check other events */
@@ -485,7 +487,7 @@ static int radeon_atcs_verify_interface(acpi_handle handle,
 
         size = *(u16 *) info->buffer.pointer;
         if (size < 8) {
-                DRM_INFO("ATCS buffer is too small: %lu\n", size);
+                DRM_INFO("ATCS buffer is too small: %zu\n", size);
                 err = -EINVAL;
                 goto out;
         }
diff --git a/drivers/gpu/drm/radeon/radeon_atpx_handler.c b/drivers/gpu/drm/radeon/radeon_atpx_handler.c
index 582e99449c12..15f5ded65e0c 100644
--- a/drivers/gpu/drm/radeon/radeon_atpx_handler.c
+++ b/drivers/gpu/drm/radeon/radeon_atpx_handler.c
@@ -87,7 +87,7 @@ static union acpi_object *radeon_atpx_call(acpi_handle handle, int function,
                 atpx_arg_elements[1].integer.value = 0;
         }
 
-        status = acpi_evaluate_object(handle, "ATPX", &atpx_arg, &buffer);
+        status = acpi_evaluate_object(handle, NULL, &atpx_arg, &buffer);
 
         /* Fail only if calling the method fails and ATPX is supported */
         if (ACPI_FAILURE(status) && status != AE_NOT_FOUND) {
@@ -148,7 +148,7 @@ static int radeon_atpx_verify_interface(struct radeon_atpx *atpx)
 
         size = *(u16 *) info->buffer.pointer;
         if (size < 8) {
-                printk("ATPX buffer is too small: %lu\n", size);
+                printk("ATPX buffer is too small: %zu\n", size);
                 err = -EINVAL;
                 goto out;
         }
@@ -352,9 +352,9 @@ static int radeon_atpx_switchto(enum vga_switcheroo_client_id id)
 }
 
 /**
- * radeon_atpx_switchto - switch to the requested GPU
+ * radeon_atpx_power_state - power down/up the requested GPU
  *
- * @id: GPU to switch to
+ * @id: GPU to power down/up
  * @state: requested power state (0 = off, 1 = on)
  *
  * Execute the necessary ATPX function to power down/up the discrete GPU
@@ -373,11 +373,11 @@ static int radeon_atpx_power_state(enum vga_switcheroo_client_id id,
 }
 
 /**
- * radeon_atpx_pci_probe_handle - look up the ATRM and ATPX handles
+ * radeon_atpx_pci_probe_handle - look up the ATPX handle
  *
  * @pdev: pci device
  *
- * Look up the ATPX and ATRM handles (all asics).
+ * Look up the ATPX handles (all asics).
  * Returns true if the handles are found, false if not.
  */
 static bool radeon_atpx_pci_probe_handle(struct pci_dev *pdev)
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 67cfc1795ecd..b884c362a8c2 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -941,7 +941,7 @@ radeon_dvi_detect(struct drm_connector *connector, bool force)
         struct drm_mode_object *obj;
         int i;
         enum drm_connector_status ret = connector_status_disconnected;
-        bool dret = false;
+        bool dret = false, broken_edid = false;
 
         if (!force && radeon_check_hpd_status_unchanged(connector))
                 return connector->status;
@@ -965,6 +965,9 @@ radeon_dvi_detect(struct drm_connector *connector, bool force)
                                 ret = connector_status_disconnected;
                                 DRM_ERROR("%s: detected RS690 floating bus bug, stopping ddc detect\n", drm_get_connector_name(connector));
                                 radeon_connector->ddc_bus = NULL;
+                        } else {
+                                ret = connector_status_connected;
+                                broken_edid = true; /* defer use_digital to later */
                         }
                 } else {
                         radeon_connector->use_digital = !!(radeon_connector->edid->input & DRM_EDID_INPUT_DIGITAL);
@@ -1047,13 +1050,24 @@ radeon_dvi_detect(struct drm_connector *connector, bool force)
 
                         encoder_funcs = encoder->helper_private;
                         if (encoder_funcs->detect) {
-                                if (ret != connector_status_connected) {
-                                        ret = encoder_funcs->detect(encoder, connector);
-                                        if (ret == connector_status_connected) {
-                                                radeon_connector->use_digital = false;
+                                if (!broken_edid) {
+                                        if (ret != connector_status_connected) {
+                                                /* deal with analog monitors without DDC */
+                                                ret = encoder_funcs->detect(encoder, connector);
+                                                if (ret == connector_status_connected) {
+                                                        radeon_connector->use_digital = false;
+                                                }
+                                                if (ret != connector_status_disconnected)
+                                                        radeon_connector->detected_by_load = true;
                                         }
-                                        if (ret != connector_status_disconnected)
-                                                radeon_connector->detected_by_load = true;
+                                } else {
+                                        enum drm_connector_status lret;
+                                        /* assume digital unless load detected otherwise */
+                                        radeon_connector->use_digital = true;
+                                        lret = encoder_funcs->detect(encoder, connector);
+                                        DRM_DEBUG_KMS("load_detect %x returned: %x\n",encoder->encoder_type,lret);
+                                        if (lret == connector_status_connected)
+                                                radeon_connector->use_digital = false;
                                 }
                                 break;
                         }
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index cb7b7c062fef..41672cc563fb 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -478,6 +478,7 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
         }
 
 out:
+        radeon_vm_add_to_lru(rdev, vm);
         mutex_unlock(&vm->mutex);
         mutex_unlock(&rdev->vm_manager.lock);
         return r;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 64a42647f08a..e2f5f888c374 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -355,6 +355,8 @@ int radeon_wb_init(struct radeon_device *rdev)
  */
 void radeon_vram_location(struct radeon_device *rdev, struct radeon_mc *mc, u64 base)
 {
+        uint64_t limit = (uint64_t)radeon_vram_limit << 20;
+
         mc->vram_start = base;
         if (mc->mc_vram_size > (0xFFFFFFFF - base + 1)) {
                 dev_warn(rdev->dev, "limiting VRAM to PCI aperture size\n");
@@ -368,8 +370,8 @@ void radeon_vram_location(struct radeon_device *rdev, struct radeon_mc *mc, u64
                 mc->mc_vram_size = mc->aper_size;
         }
         mc->vram_end = mc->vram_start + mc->mc_vram_size - 1;
-        if (radeon_vram_limit && radeon_vram_limit < mc->real_vram_size)
-                mc->real_vram_size = radeon_vram_limit;
+        if (limit && limit < mc->real_vram_size)
+                mc->real_vram_size = limit;
         dev_info(rdev->dev, "VRAM: %lluM 0x%016llX - 0x%016llX (%lluM used)\n",
                         mc->mc_vram_size >> 20, mc->vram_start,
                         mc->vram_end, mc->real_vram_size >> 20);
@@ -835,6 +837,19 @@ static unsigned int radeon_vga_set_decode(void *cookie, bool state)
 }
 
 /**
+ * radeon_check_pot_argument - check that argument is a power of two
+ *
+ * @arg: value to check
+ *
+ * Validates that a certain argument is a power of two (all asics).
+ * Returns true if argument is valid.
+ */
+static bool radeon_check_pot_argument(int arg)
+{
+        return (arg & (arg - 1)) == 0;
+}
+
+/**
  * radeon_check_arguments - validate module params
  *
  * @rdev: radeon_device pointer
@@ -845,52 +860,25 @@ static unsigned int radeon_vga_set_decode(void *cookie, bool state)
 static void radeon_check_arguments(struct radeon_device *rdev)
 {
         /* vramlimit must be a power of two */
-        switch (radeon_vram_limit) {
-        case 0:
-        case 4:
-        case 8:
-        case 16:
-        case 32:
-        case 64:
-        case 128:
-        case 256:
-        case 512:
-        case 1024:
-        case 2048:
-        case 4096:
-                break;
-        default:
+        if (!radeon_check_pot_argument(radeon_vram_limit)) {
                 dev_warn(rdev->dev, "vram limit (%d) must be a power of 2\n",
                                 radeon_vram_limit);
                 radeon_vram_limit = 0;
-                break;
         }
-        radeon_vram_limit = radeon_vram_limit << 20;
+
         /* gtt size must be power of two and greater or equal to 32M */
-        switch (radeon_gart_size) {
-        case 4:
-        case 8:
-        case 16:
+        if (radeon_gart_size < 32) {
                 dev_warn(rdev->dev, "gart size (%d) too small forcing to 512M\n",
                                 radeon_gart_size);
                 radeon_gart_size = 512;
-                break;
-        case 32:
-        case 64:
-        case 128:
-        case 256:
-        case 512:
-        case 1024:
-        case 2048:
-        case 4096:
-                break;
-        default:
+
+        } else if (!radeon_check_pot_argument(radeon_gart_size)) {
                 dev_warn(rdev->dev, "gart size (%d) must be a power of 2\n",
                                 radeon_gart_size);
                 radeon_gart_size = 512;
-                break;
         }
-        rdev->mc.gtt_size = radeon_gart_size * 1024 * 1024;
+        rdev->mc.gtt_size = (uint64_t)radeon_gart_size << 20;
+
         /* AGP mode can only be -1, 1, 2, 4, 8 */
         switch (radeon_agpmode) {
         case -1:
@@ -1018,6 +1006,10 @@ int radeon_device_init(struct radeon_device *rdev,
                 return r;
         /* initialize vm here */
         mutex_init(&rdev->vm_manager.lock);
+        /* Adjust VM size here.
+         * Currently set to 4GB ((1 << 20) 4k pages).
+         * Max GPUVM size for cayman and SI is 40 bits.
+         */
         rdev->vm_manager.max_pfn = 1 << 20;
         INIT_LIST_HEAD(&rdev->vm_manager.lru_vm);
 
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index f0c06d196b75..4debd60e5aa6 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -355,14 +355,13 @@ int radeon_gart_init(struct radeon_device *rdev)
         DRM_INFO("GART: num cpu pages %u, num gpu pages %u\n",
                  rdev->gart.num_cpu_pages, rdev->gart.num_gpu_pages);
         /* Allocate pages table */
-        rdev->gart.pages = kzalloc(sizeof(void *) * rdev->gart.num_cpu_pages,
-                                   GFP_KERNEL);
+        rdev->gart.pages = vzalloc(sizeof(void *) * rdev->gart.num_cpu_pages);
         if (rdev->gart.pages == NULL) {
                 radeon_gart_fini(rdev);
                 return -ENOMEM;
         }
-        rdev->gart.pages_addr = kzalloc(sizeof(dma_addr_t) *
-                                        rdev->gart.num_cpu_pages, GFP_KERNEL);
+        rdev->gart.pages_addr = vzalloc(sizeof(dma_addr_t) *
+                                        rdev->gart.num_cpu_pages);
         if (rdev->gart.pages_addr == NULL) {
                 radeon_gart_fini(rdev);
                 return -ENOMEM;
@@ -388,8 +387,8 @@ void radeon_gart_fini(struct radeon_device *rdev)
                 radeon_gart_unbind(rdev, 0, rdev->gart.num_cpu_pages);
         }
         rdev->gart.ready = false;
-        kfree(rdev->gart.pages);
-        kfree(rdev->gart.pages_addr);
+        vfree(rdev->gart.pages);
+        vfree(rdev->gart.pages_addr);
         rdev->gart.pages = NULL;
         rdev->gart.pages_addr = NULL;
 
@@ -423,6 +422,18 @@ void radeon_gart_fini(struct radeon_device *rdev)
  */
 
 /**
+ * radeon_vm_num_pde - return the number of page directory entries
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Calculate the number of page directory entries (cayman+).
+ */
+static unsigned radeon_vm_num_pdes(struct radeon_device *rdev)
+{
+        return rdev->vm_manager.max_pfn >> RADEON_VM_BLOCK_SIZE;
+}
+
+/**
  * radeon_vm_directory_size - returns the size of the page directory in bytes
  *
  * @rdev: radeon_device pointer
@@ -431,7 +442,7 @@ void radeon_gart_fini(struct radeon_device *rdev)
  */
 static unsigned radeon_vm_directory_size(struct radeon_device *rdev)
 {
-        return (rdev->vm_manager.max_pfn >> RADEON_VM_BLOCK_SIZE) * 8;
+        return RADEON_GPU_PAGE_ALIGN(radeon_vm_num_pdes(rdev) * 8);
 }
 
 /**
@@ -451,11 +462,11 @@ int radeon_vm_manager_init(struct radeon_device *rdev)
 
         if (!rdev->vm_manager.enabled) {
                 /* allocate enough for 2 full VM pts */
-                size = RADEON_GPU_PAGE_ALIGN(radeon_vm_directory_size(rdev));
-                size += RADEON_GPU_PAGE_ALIGN(rdev->vm_manager.max_pfn * 8);
+                size = radeon_vm_directory_size(rdev);
+                size += rdev->vm_manager.max_pfn * 8;
                 size *= 2;
                 r = radeon_sa_bo_manager_init(rdev, &rdev->vm_manager.sa_manager,
-                                              size,
+                                              RADEON_GPU_PAGE_ALIGN(size),
                                               RADEON_GEM_DOMAIN_VRAM);
                 if (r) {
                         dev_err(rdev->dev, "failed to allocate vm bo (%dKB)\n",
@@ -476,7 +487,7 @@ int radeon_vm_manager_init(struct radeon_device *rdev)
 
         /* restore page table */
         list_for_each_entry(vm, &rdev->vm_manager.lru_vm, list) {
-                if (vm->sa_bo == NULL)
+                if (vm->page_directory == NULL)
                         continue;
 
                 list_for_each_entry(bo_va, &vm->va, vm_list) {
@@ -500,16 +511,25 @@ static void radeon_vm_free_pt(struct radeon_device *rdev,
                                     struct radeon_vm *vm)
 {
         struct radeon_bo_va *bo_va;
+        int i;
 
-        if (!vm->sa_bo)
+        if (!vm->page_directory)
                 return;
 
         list_del_init(&vm->list);
-        radeon_sa_bo_free(rdev, &vm->sa_bo, vm->fence);
+        radeon_sa_bo_free(rdev, &vm->page_directory, vm->fence);
 
         list_for_each_entry(bo_va, &vm->va, vm_list) {
                 bo_va->valid = false;
         }
+
+        if (vm->page_tables == NULL)
+                return;
+
+        for (i = 0; i < radeon_vm_num_pdes(rdev); i++)
+                radeon_sa_bo_free(rdev, &vm->page_tables[i], vm->fence);
+
+        kfree(vm->page_tables);
 }
 
 /**
@@ -546,63 +566,106 @@ void radeon_vm_manager_fini(struct radeon_device *rdev)
 }
 
 /**
+ * radeon_vm_evict - evict page table to make room for new one
+ *
+ * @rdev: radeon_device pointer
+ * @vm: VM we want to allocate something for
+ *
+ * Evict a VM from the lru, making sure that it isn't @vm. (cayman+).
+ * Returns 0 for success, -ENOMEM for failure.
+ *
+ * Global and local mutex must be locked!
+ */
+static int radeon_vm_evict(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+        struct radeon_vm *vm_evict;
+
+        if (list_empty(&rdev->vm_manager.lru_vm))
+                return -ENOMEM;
+
+        vm_evict = list_first_entry(&rdev->vm_manager.lru_vm,
+                                    struct radeon_vm, list);
+        if (vm_evict == vm)
+                return -ENOMEM;
+
+        mutex_lock(&vm_evict->mutex);
+        radeon_vm_free_pt(rdev, vm_evict);
+        mutex_unlock(&vm_evict->mutex);
+        return 0;
+}
+
+/**
  * radeon_vm_alloc_pt - allocates a page table for a VM
  *
  * @rdev: radeon_device pointer
  * @vm: vm to bind
  *
  * Allocate a page table for the requested vm (cayman+).
- * Also starts to populate the page table.
  * Returns 0 for success, error for failure.
  *
  * Global and local mutex must be locked!
  */
 int radeon_vm_alloc_pt(struct radeon_device *rdev, struct radeon_vm *vm)
 {
-        struct radeon_vm *vm_evict;
-        int r;
+        unsigned pd_size, pts_size;
         u64 *pd_addr;
-        int tables_size;
+        int r;
 
         if (vm == NULL) {
                 return -EINVAL;
         }
 
-        /* allocate enough to cover the current VM size */
-        tables_size = RADEON_GPU_PAGE_ALIGN(radeon_vm_directory_size(rdev));
-        tables_size += RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8);
-
-        if (vm->sa_bo != NULL) {
-                /* update lru */
-                list_del_init(&vm->list);
-                list_add_tail(&vm->list, &rdev->vm_manager.lru_vm);
+        if (vm->page_directory != NULL) {
                 return 0;
         }
 
 retry:
-        r = radeon_sa_bo_new(rdev, &rdev->vm_manager.sa_manager, &vm->sa_bo,
-                             tables_size, RADEON_GPU_PAGE_SIZE, false);
+        pd_size = RADEON_GPU_PAGE_ALIGN(radeon_vm_directory_size(rdev));
+        r = radeon_sa_bo_new(rdev, &rdev->vm_manager.sa_manager,
+                             &vm->page_directory, pd_size,
+                             RADEON_GPU_PAGE_SIZE, false);
         if (r == -ENOMEM) {
-                if (list_empty(&rdev->vm_manager.lru_vm)) {
+                r = radeon_vm_evict(rdev, vm);
+                if (r)
                         return r;
-                }
-                vm_evict = list_first_entry(&rdev->vm_manager.lru_vm, struct radeon_vm, list);
-                mutex_lock(&vm_evict->mutex);
-                radeon_vm_free_pt(rdev, vm_evict);
-                mutex_unlock(&vm_evict->mutex);
                 goto retry;
 
         } else if (r) {
                 return r;
         }
 
-        pd_addr = radeon_sa_bo_cpu_addr(vm->sa_bo);
-        vm->pd_gpu_addr = radeon_sa_bo_gpu_addr(vm->sa_bo);
-        memset(pd_addr, 0, tables_size);
+        vm->pd_gpu_addr = radeon_sa_bo_gpu_addr(vm->page_directory);
+
+        /* Initially clear the page directory */
+        pd_addr = radeon_sa_bo_cpu_addr(vm->page_directory);
+        memset(pd_addr, 0, pd_size);
+
+        pts_size = radeon_vm_num_pdes(rdev) * sizeof(struct radeon_sa_bo *);
+        vm->page_tables = kzalloc(pts_size, GFP_KERNEL);
+
+        if (vm->page_tables == NULL) {
+                DRM_ERROR("Cannot allocate memory for page table array\n");
+                radeon_sa_bo_free(rdev, &vm->page_directory, vm->fence);
+                return -ENOMEM;
+        }
+
+        return 0;
+}
 
+/**
+ * radeon_vm_add_to_lru - add VMs page table to LRU list
+ *
+ * @rdev: radeon_device pointer
+ * @vm: vm to add to LRU
+ *
+ * Add the allocated page table to the LRU list (cayman+).
+ *
+ * Global mutex must be locked!
+ */
+void radeon_vm_add_to_lru(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+        list_del_init(&vm->list);
         list_add_tail(&vm->list, &rdev->vm_manager.lru_vm);
-        return radeon_vm_bo_update_pte(rdev, vm, rdev->ring_tmp_bo.bo,
-                                       &rdev->ring_tmp_bo.bo->tbo.mem);
 }
 
 /**
@@ -793,20 +856,6 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
         }
 
         mutex_lock(&vm->mutex);
-        if (last_pfn > vm->last_pfn) {
-                /* release mutex and lock in right order */
-                mutex_unlock(&vm->mutex);
-                mutex_lock(&rdev->vm_manager.lock);
-                mutex_lock(&vm->mutex);
-                /* and check again */
-                if (last_pfn > vm->last_pfn) {
-                        /* grow va space 32M by 32M */
-                        unsigned align = ((32 << 20) >> 12) - 1;
-                        radeon_vm_free_pt(rdev, vm);
-                        vm->last_pfn = (last_pfn + align) & ~align;
-                }
-                mutex_unlock(&rdev->vm_manager.lock);
-        }
         head = &vm->va;
         last_offset = 0;
         list_for_each_entry(tmp, &vm->va, vm_list) {
@@ -865,6 +914,154 @@ uint64_t radeon_vm_map_gart(struct radeon_device *rdev, uint64_t addr)
 }
 
 /**
+ * radeon_vm_update_pdes - make sure that page directory is valid
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ * @start: start of GPU address range
+ * @end: end of GPU address range
+ *
+ * Allocates new page tables if necessary
+ * and updates the page directory (cayman+).
+ * Returns 0 for success, error for failure.
+ *
+ * Global and local mutex must be locked!
+ */
+static int radeon_vm_update_pdes(struct radeon_device *rdev,
+                                 struct radeon_vm *vm,
+                                 uint64_t start, uint64_t end)
+{
+        static const uint32_t incr = RADEON_VM_PTE_COUNT * 8;
+
+        uint64_t last_pde = ~0, last_pt = ~0;
+        unsigned count = 0;
+        uint64_t pt_idx;
+        int r;
+
+        start = (start / RADEON_GPU_PAGE_SIZE) >> RADEON_VM_BLOCK_SIZE;
+        end = (end / RADEON_GPU_PAGE_SIZE) >> RADEON_VM_BLOCK_SIZE;
+
+        /* walk over the address space and update the page directory */
+        for (pt_idx = start; pt_idx <= end; ++pt_idx) {
+                uint64_t pde, pt;
+
+                if (vm->page_tables[pt_idx])
+                        continue;
+
+retry:
+                r = radeon_sa_bo_new(rdev, &rdev->vm_manager.sa_manager,
+                                     &vm->page_tables[pt_idx],
+                                     RADEON_VM_PTE_COUNT * 8,
+                                     RADEON_GPU_PAGE_SIZE, false);
+
+                if (r == -ENOMEM) {
+                        r = radeon_vm_evict(rdev, vm);
+                        if (r)
+                                return r;
+                        goto retry;
+                } else if (r) {
+                        return r;
+                }
+
+                pde = vm->pd_gpu_addr + pt_idx * 8;
+
+                pt = radeon_sa_bo_gpu_addr(vm->page_tables[pt_idx]);
+
+                if (((last_pde + 8 * count) != pde) ||
+                    ((last_pt + incr * count) != pt)) {
+
+                        if (count) {
+                                radeon_asic_vm_set_page(rdev, last_pde,
+                                                        last_pt, count, incr,
+                                                        RADEON_VM_PAGE_VALID);
+                        }
+
+                        count = 1;
+                        last_pde = pde;
+                        last_pt = pt;
+                } else {
+                        ++count;
+                }
+        }
+
+        if (count) {
+                radeon_asic_vm_set_page(rdev, last_pde, last_pt, count,
+                                        incr, RADEON_VM_PAGE_VALID);
+
+        }
+
+        return 0;
+}
+
+/**
+ * radeon_vm_update_ptes - make sure that page tables are valid
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ * @start: start of GPU address range
+ * @end: end of GPU address range
+ * @dst: destination address to map to
+ * @flags: mapping flags
+ *
+ * Update the page tables in the range @start - @end (cayman+).
+ *
+ * Global and local mutex must be locked!
+ */
+static void radeon_vm_update_ptes(struct radeon_device *rdev,
+                                  struct radeon_vm *vm,
+                                  uint64_t start, uint64_t end,
+                                  uint64_t dst, uint32_t flags)
+{
+        static const uint64_t mask = RADEON_VM_PTE_COUNT - 1;
+
+        uint64_t last_pte = ~0, last_dst = ~0;
+        unsigned count = 0;
+        uint64_t addr;
+
+        start = start / RADEON_GPU_PAGE_SIZE;
+        end = end / RADEON_GPU_PAGE_SIZE;
+
+        /* walk over the address space and update the page tables */
+        for (addr = start; addr < end; ) {
+                uint64_t pt_idx = addr >> RADEON_VM_BLOCK_SIZE;
+                unsigned nptes;
+                uint64_t pte;
+
+                if ((addr & ~mask) == (end & ~mask))
+                        nptes = end - addr;
+                else
+                        nptes = RADEON_VM_PTE_COUNT - (addr & mask);
+
+                pte = radeon_sa_bo_gpu_addr(vm->page_tables[pt_idx]);
+                pte += (addr & mask) * 8;
+
+                if ((last_pte + 8 * count) != pte) {
+
+                        if (count) {
+                                radeon_asic_vm_set_page(rdev, last_pte,
+                                                        last_dst, count,
+                                                        RADEON_GPU_PAGE_SIZE,
+                                                        flags);
+                        }
+
+                        count = nptes;
+                        last_pte = pte;
+                        last_dst = dst;
+                } else {
+                        count += nptes;
+                }
+
+                addr += nptes;
+                dst += nptes * RADEON_GPU_PAGE_SIZE;
+        }
+
+        if (count) {
+                radeon_asic_vm_set_page(rdev, last_pte, last_dst, count,
+                                        RADEON_GPU_PAGE_SIZE, flags);
+        }
+}
+
+/**
  * radeon_vm_bo_update_pte - map a bo into the vm page table
  *
  * @rdev: radeon_device pointer
@@ -887,12 +1084,11 @@ int radeon_vm_bo_update_pte(struct radeon_device *rdev,
         struct radeon_semaphore *sem = NULL;
         struct radeon_bo_va *bo_va;
         unsigned nptes, npdes, ndw;
-        uint64_t pe, addr;
-        uint64_t pfn;
+        uint64_t addr;
         int r;
 
         /* nothing to do if vm isn't bound */
-        if (vm->sa_bo == NULL)
+        if (vm->page_directory == NULL)
                 return 0;
 
         bo_va = radeon_vm_bo_find(vm, bo);
@@ -939,25 +1135,29 @@ int radeon_vm_bo_update_pte(struct radeon_device *rdev,
                 }
         }
 
-        /* estimate number of dw needed */
-        /* reserve space for 32-bit padding */
-        ndw = 32;
-
         nptes = radeon_bo_ngpu_pages(bo);
 
-        pfn = (bo_va->soffset / RADEON_GPU_PAGE_SIZE);
+        /* assume two extra pdes in case the mapping overlaps the borders */
+        npdes = (nptes >> RADEON_VM_BLOCK_SIZE) + 2;
 
-        /* handle cases where a bo spans several pdes  */
-        npdes = (ALIGN(pfn + nptes, RADEON_VM_PTE_COUNT) -
-                 (pfn & ~(RADEON_VM_PTE_COUNT - 1))) >> RADEON_VM_BLOCK_SIZE;
+        /* estimate number of dw needed */
+        /* semaphore, fence and padding */
+        ndw = 32;
+
+        if (RADEON_VM_BLOCK_SIZE > 11)
+                /* reserve space for one header for every 2k dwords */
+                ndw += (nptes >> 11) * 4;
+        else
+                /* reserve space for one header for
+                    every (1 << BLOCK_SIZE) entries */
+                ndw += (nptes >> RADEON_VM_BLOCK_SIZE) * 4;
 
-        /* reserve space for one header for every 2k dwords */
-        ndw += (nptes >> 11) * 3;
         /* reserve space for pte addresses */
         ndw += nptes * 2;
 
         /* reserve space for one header for every 2k dwords */
-        ndw += (npdes >> 11) * 3;
+        ndw += (npdes >> 11) * 4;
+
         /* reserve space for pde addresses */
         ndw += npdes * 2;
 
@@ -971,22 +1171,14 @@ int radeon_vm_bo_update_pte(struct radeon_device *rdev,
                 radeon_fence_note_sync(vm->fence, ridx);
         }
 
-        /* update page table entries */
-        pe = vm->pd_gpu_addr;
-        pe += radeon_vm_directory_size(rdev);
-        pe += (bo_va->soffset / RADEON_GPU_PAGE_SIZE) * 8;
-
-        radeon_asic_vm_set_page(rdev, pe, addr, nptes,
-                                RADEON_GPU_PAGE_SIZE, bo_va->flags);
-
-        /* update page directory entries */
-        addr = pe;
-
-        pe = vm->pd_gpu_addr;
-        pe += ((bo_va->soffset / RADEON_GPU_PAGE_SIZE) >> RADEON_VM_BLOCK_SIZE) * 8;
+        r = radeon_vm_update_pdes(rdev, vm, bo_va->soffset, bo_va->eoffset);
+        if (r) {
+                radeon_ring_unlock_undo(rdev, ring);
+                return r;
+        }
 
-        radeon_asic_vm_set_page(rdev, pe, addr, npdes,
-                                RADEON_VM_PTE_COUNT * 8, RADEON_VM_PAGE_VALID);
+        radeon_vm_update_ptes(rdev, vm, bo_va->soffset, bo_va->eoffset,
+                              addr, bo_va->flags);
 
         radeon_fence_unref(&vm->fence);
         r = radeon_fence_emit(rdev, &vm->fence, ridx);
@@ -997,6 +1189,7 @@ int radeon_vm_bo_update_pte(struct radeon_device *rdev,
         radeon_ring_unlock_commit(rdev, ring);
         radeon_semaphore_free(rdev, &sem, vm->fence);
         radeon_fence_unref(&vm->last_flush);
+
         return 0;
 }
 
@@ -1056,31 +1249,15 @@ void radeon_vm_bo_invalidate(struct radeon_device *rdev,
  * @rdev: radeon_device pointer
  * @vm: requested vm
  *
- * Init @vm (cayman+).
- * Map the IB pool and any other shared objects into the VM
- * by default as it's used by all VMs.
- * Returns 0 for success, error for failure.
+ * Init @vm fields (cayman+).
  */
-int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
+void radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 {
-        struct radeon_bo_va *bo_va;
-        int r;
-
         vm->id = 0;
         vm->fence = NULL;
-        vm->last_pfn = 0;
         mutex_init(&vm->mutex);
         INIT_LIST_HEAD(&vm->list);
         INIT_LIST_HEAD(&vm->va);
-
-        /* map the ib pool buffer at 0 in virtual address space, set
-         * read only
-         */
-        bo_va = radeon_vm_bo_add(rdev, vm, rdev->ring_tmp_bo.bo);
-        r = radeon_vm_bo_set_addr(rdev, bo_va, RADEON_VA_IB_OFFSET,
-                                  RADEON_VM_PAGE_READABLE |
-                                  RADEON_VM_PAGE_SNOOPED);
-        return r;
 }
 
 /**
@@ -1102,17 +1279,6 @@ void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
         radeon_vm_free_pt(rdev, vm);
         mutex_unlock(&rdev->vm_manager.lock);
 
-        /* remove all bo at this point non are busy any more because unbind
-         * waited for the last vm fence to signal
-         */
-        r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
-        if (!r) {
-                bo_va = radeon_vm_bo_find(vm, rdev->ring_tmp_bo.bo);
-                list_del_init(&bo_va->bo_list);
-                list_del_init(&bo_va->vm_list);
-                radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
-                kfree(bo_va);
-        }
         if (!list_empty(&vm->va)) {
                 dev_err(rdev->dev, "still active bo inside vm\n");
         }
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index f38fbcc46935..fe5c1f6b7957 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -53,6 +53,7 @@ int radeon_gem_object_create(struct radeon_device *rdev, int size,
                                 struct drm_gem_object **obj)
 {
         struct radeon_bo *robj;
+        unsigned long max_size;
         int r;
 
         *obj = NULL;
@@ -60,11 +61,26 @@ int radeon_gem_object_create(struct radeon_device *rdev, int size,
         if (alignment < PAGE_SIZE) {
                 alignment = PAGE_SIZE;
         }
+
+        /* maximun bo size is the minimun btw visible vram and gtt size */
+        max_size = min(rdev->mc.visible_vram_size, rdev->mc.gtt_size);
+        if (size > max_size) {
+                printk(KERN_WARNING "%s:%d alloc size %dMb bigger than %ldMb limit\n",
+                       __func__, __LINE__, size >> 20, max_size >> 20);
+                return -ENOMEM;
+        }
+
+retry:
         r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain, NULL, &robj);
         if (r) {
-                if (r != -ERESTARTSYS)
+                if (r != -ERESTARTSYS) {
+                        if (initial_domain == RADEON_GEM_DOMAIN_VRAM) {
+                                initial_domain |= RADEON_GEM_DOMAIN_GTT;
+                                goto retry;
+                        }
                         DRM_ERROR("Failed to allocate GEM object (%d, %d, %u, %d)\n",
                                   size, initial_domain, alignment, r);
+                }
                 return r;
         }
         *obj = &robj->gem_base;
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 83b8d8aa71c0..dc781c49b96b 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -419,6 +419,7 @@ int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
         /* new gpu have virtual address space support */
         if (rdev->family >= CHIP_CAYMAN) {
                 struct radeon_fpriv *fpriv;
+                struct radeon_bo_va *bo_va;
                 int r;
 
                 fpriv = kzalloc(sizeof(*fpriv), GFP_KERNEL);
@@ -426,7 +427,15 @@ int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
                         return -ENOMEM;
                 }
 
-                r = radeon_vm_init(rdev, &fpriv->vm);
+                radeon_vm_init(rdev, &fpriv->vm);
+
+                /* map the ib pool buffer read only into
+                 * virtual address space */
+                bo_va = radeon_vm_bo_add(rdev, &fpriv->vm,
+                                         rdev->ring_tmp_bo.bo);
+                r = radeon_vm_bo_set_addr(rdev, bo_va, RADEON_VA_IB_OFFSET,
+                                          RADEON_VM_PAGE_READABLE |
+                                          RADEON_VM_PAGE_SNOOPED);
                 if (r) {
                         radeon_vm_fini(rdev, &fpriv->vm);
                         kfree(fpriv);
@@ -454,6 +463,17 @@ void radeon_driver_postclose_kms(struct drm_device *dev,
         /* new gpu have virtual address space support */
         if (rdev->family >= CHIP_CAYMAN && file_priv->driver_priv) {
                 struct radeon_fpriv *fpriv = file_priv->driver_priv;
+                struct radeon_bo_va *bo_va;
+                int r;
+
+                r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
+                if (!r) {
+                        bo_va = radeon_vm_bo_find(&fpriv->vm,
+                                                  rdev->ring_tmp_bo.bo);
+                        if (bo_va)
+                                radeon_vm_bo_rmv(rdev, bo_va);
+                        radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+                }
 
                 radeon_vm_fini(rdev, &fpriv->vm);
                 kfree(fpriv);
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index 5677a424b585..6857cb4efb76 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -295,6 +295,7 @@ static void radeon_crtc_dpms(struct drm_crtc *crtc, int mode)
         struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
         struct drm_device *dev = crtc->dev;
         struct radeon_device *rdev = dev->dev_private;
+        uint32_t crtc_ext_cntl = 0;
         uint32_t mask;
 
         if (radeon_crtc->crtc_id)
@@ -307,6 +308,16 @@ static void radeon_crtc_dpms(struct drm_crtc *crtc, int mode)
                         RADEON_CRTC_VSYNC_DIS |
                         RADEON_CRTC_HSYNC_DIS);
 
+        /*
+         * On all dual CRTC GPUs this bit controls the CRTC of the primary DAC.
+         * Therefore it is set in the DAC DMPS function.
+         * This is different for GPU's with a single CRTC but a primary and a
+         * TV DAC: here it controls the single CRTC no matter where it is
+         * routed. Therefore we set it here.
+         */
+        if (rdev->flags & RADEON_SINGLE_CRTC)
+                crtc_ext_cntl = RADEON_CRTC_CRT_ON;
+        
         switch (mode) {
         case DRM_MODE_DPMS_ON:
                 radeon_crtc->enabled = true;
@@ -317,7 +328,7 @@ static void radeon_crtc_dpms(struct drm_crtc *crtc, int mode)
                 else {
                         WREG32_P(RADEON_CRTC_GEN_CNTL, RADEON_CRTC_EN, ~(RADEON_CRTC_EN |
                                                                          RADEON_CRTC_DISP_REQ_EN_B));
-                        WREG32_P(RADEON_CRTC_EXT_CNTL, 0, ~mask);
+                        WREG32_P(RADEON_CRTC_EXT_CNTL, crtc_ext_cntl, ~(mask | crtc_ext_cntl));
                 }
                 drm_vblank_post_modeset(dev, radeon_crtc->crtc_id);
                 radeon_crtc_load_lut(crtc);
@@ -331,7 +342,7 @@ static void radeon_crtc_dpms(struct drm_crtc *crtc, int mode)
                 else {
                         WREG32_P(RADEON_CRTC_GEN_CNTL, RADEON_CRTC_DISP_REQ_EN_B, ~(RADEON_CRTC_EN |
                                                                                     RADEON_CRTC_DISP_REQ_EN_B));
-                        WREG32_P(RADEON_CRTC_EXT_CNTL, mask, ~mask);
+                        WREG32_P(RADEON_CRTC_EXT_CNTL, mask, ~(mask | crtc_ext_cntl));
                 }
                 radeon_crtc->enabled = false;
                 /* adjust pm to dpms changes AFTER disabling crtcs */
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
index 92487e614778..f5ba2241dacc 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
@@ -269,27 +269,6 @@ static const struct drm_encoder_helper_funcs radeon_legacy_lvds_helper_funcs = {
         .disable = radeon_legacy_encoder_disable,
 };
 
-#if defined(CONFIG_BACKLIGHT_CLASS_DEVICE) || defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE)
-
-static uint8_t radeon_legacy_lvds_level(struct backlight_device *bd)
-{
-        struct radeon_backlight_privdata *pdata = bl_get_data(bd);
-        uint8_t level;
-
-        /* Convert brightness to hardware level */
-        if (bd->props.brightness < 0)
-                level = 0;
-        else if (bd->props.brightness > RADEON_MAX_BL_LEVEL)
-                level = RADEON_MAX_BL_LEVEL;
-        else
-                level = bd->props.brightness;
-
-        if (pdata->negative)
-                level = RADEON_MAX_BL_LEVEL - level;
-
-        return level;
-}
-
 u8
 radeon_legacy_get_backlight_level(struct radeon_encoder *radeon_encoder)
 {
@@ -331,6 +310,27 @@ radeon_legacy_set_backlight_level(struct radeon_encoder *radeon_encoder, u8 leve
         radeon_legacy_lvds_update(&radeon_encoder->base, dpms_mode);
 }
 
+#if defined(CONFIG_BACKLIGHT_CLASS_DEVICE) || defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE)
+
+static uint8_t radeon_legacy_lvds_level(struct backlight_device *bd)
+{
+        struct radeon_backlight_privdata *pdata = bl_get_data(bd);
+        uint8_t level;
+
+        /* Convert brightness to hardware level */
+        if (bd->props.brightness < 0)
+                level = 0;
+        else if (bd->props.brightness > RADEON_MAX_BL_LEVEL)
+                level = RADEON_MAX_BL_LEVEL;
+        else
+                level = bd->props.brightness;
+
+        if (pdata->negative)
+                level = RADEON_MAX_BL_LEVEL - level;
+
+        return level;
+}
+
 static int radeon_legacy_backlight_update_status(struct backlight_device *bd)
 {
         struct radeon_backlight_privdata *pdata = bl_get_data(bd);
@@ -370,6 +370,7 @@ void radeon_legacy_backlight_init(struct radeon_encoder *radeon_encoder,
         struct backlight_properties props;
         struct radeon_backlight_privdata *pdata;
         uint8_t backlight_level;
+        char bl_name[16];
 
         if (!radeon_encoder->enc_priv)
                 return;
@@ -389,7 +390,9 @@ void radeon_legacy_backlight_init(struct radeon_encoder *radeon_encoder,
         memset(&props, 0, sizeof(props));
         props.max_brightness = RADEON_MAX_BL_LEVEL;
         props.type = BACKLIGHT_RAW;
-        bd = backlight_device_register("radeon_bl", &drm_connector->kdev,
+        snprintf(bl_name, sizeof(bl_name),
+                 "radeon_bl%d", dev->primary->index);
+        bd = backlight_device_register(bl_name, &drm_connector->kdev,
                                        pdata, &radeon_backlight_ops, &props);
         if (IS_ERR(bd)) {
                 DRM_ERROR("Backlight registration failed\n");
@@ -534,7 +537,9 @@ static void radeon_legacy_primary_dac_dpms(struct drm_encoder *encoder, int mode
                 break;
         }
 
-        WREG32(RADEON_CRTC_EXT_CNTL, crtc_ext_cntl);
+        /* handled in radeon_crtc_dpms() */
+        if (!(rdev->flags & RADEON_SINGLE_CRTC))
+                WREG32(RADEON_CRTC_EXT_CNTL, crtc_ext_cntl);
         WREG32(RADEON_DAC_CNTL, dac_cntl);
         WREG32(RADEON_DAC_MACRO_CNTL, dac_macro_cntl);
 
@@ -659,6 +664,8 @@ static enum drm_connector_status radeon_legacy_primary_dac_detect(struct drm_enc
 
         if (ASIC_IS_R300(rdev))
                 tmp |= (0x1b6 << RADEON_DAC_FORCE_DATA_SHIFT);
+        else if (ASIC_IS_RV100(rdev))
+                tmp |= (0x1ac << RADEON_DAC_FORCE_DATA_SHIFT);
         else
                 tmp |= (0x180 << RADEON_DAC_FORCE_DATA_SHIFT);
 
@@ -668,6 +675,7 @@ static enum drm_connector_status radeon_legacy_primary_dac_detect(struct drm_enc
         tmp |= RADEON_DAC_RANGE_CNTL_PS2 | RADEON_DAC_CMP_EN;
         WREG32(RADEON_DAC_CNTL, tmp);
 
+        tmp = dac_macro_cntl;
         tmp &= ~(RADEON_DAC_PDWN_R |
                  RADEON_DAC_PDWN_G |
                  RADEON_DAC_PDWN_B);
@@ -991,11 +999,7 @@ static void radeon_legacy_tmds_ext_mode_set(struct drm_encoder *encoder,
 static void radeon_ext_tmds_enc_destroy(struct drm_encoder *encoder)
 {
         struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-        struct radeon_encoder_ext_tmds *tmds = radeon_encoder->enc_priv;
-        if (tmds) {
-                if (tmds->i2c_bus)
-                        radeon_i2c_destroy(tmds->i2c_bus);
-        }
+        /* don't destroy the i2c bus record here, this will be done in radeon_i2c_fini */
         kfree(radeon_encoder->enc_priv);
         drm_encoder_cleanup(encoder);
         kfree(radeon_encoder);
@@ -1093,7 +1097,8 @@ static void radeon_legacy_tv_dac_dpms(struct drm_encoder *encoder, int mode)
         } else {
                 if (is_tv)
                         WREG32(RADEON_TV_MASTER_CNTL, tv_master_cntl);
-                else
+                /* handled in radeon_crtc_dpms() */
+                else if (!(rdev->flags & RADEON_SINGLE_CRTC))
                         WREG32(RADEON_CRTC2_GEN_CNTL, crtc2_gen_cntl);
                 WREG32(RADEON_TV_DAC_CNTL, tv_dac_cntl);
         }
@@ -1417,13 +1422,104 @@ static bool radeon_legacy_tv_detect(struct drm_encoder *encoder,
         return found;
 }
 
+static bool radeon_legacy_ext_dac_detect(struct drm_encoder *encoder,
+                                         struct drm_connector *connector)
+{
+        struct drm_device *dev = encoder->dev;
+        struct radeon_device *rdev = dev->dev_private;
+        uint32_t gpio_monid, fp2_gen_cntl, disp_output_cntl, crtc2_gen_cntl;
+        uint32_t disp_lin_trans_grph_a, disp_lin_trans_grph_b, disp_lin_trans_grph_c;
+        uint32_t disp_lin_trans_grph_d, disp_lin_trans_grph_e, disp_lin_trans_grph_f;
+        uint32_t tmp, crtc2_h_total_disp, crtc2_v_total_disp;
+        uint32_t crtc2_h_sync_strt_wid, crtc2_v_sync_strt_wid;
+        bool found = false;
+        int i;
+
+        /* save the regs we need */
+        gpio_monid = RREG32(RADEON_GPIO_MONID);
+        fp2_gen_cntl = RREG32(RADEON_FP2_GEN_CNTL);
+        disp_output_cntl = RREG32(RADEON_DISP_OUTPUT_CNTL);
+        crtc2_gen_cntl = RREG32(RADEON_CRTC2_GEN_CNTL);
+        disp_lin_trans_grph_a = RREG32(RADEON_DISP_LIN_TRANS_GRPH_A);
+        disp_lin_trans_grph_b = RREG32(RADEON_DISP_LIN_TRANS_GRPH_B);
+        disp_lin_trans_grph_c = RREG32(RADEON_DISP_LIN_TRANS_GRPH_C);
+        disp_lin_trans_grph_d = RREG32(RADEON_DISP_LIN_TRANS_GRPH_D);
+        disp_lin_trans_grph_e = RREG32(RADEON_DISP_LIN_TRANS_GRPH_E);
+        disp_lin_trans_grph_f = RREG32(RADEON_DISP_LIN_TRANS_GRPH_F);
+        crtc2_h_total_disp = RREG32(RADEON_CRTC2_H_TOTAL_DISP);
+        crtc2_v_total_disp = RREG32(RADEON_CRTC2_V_TOTAL_DISP);
+        crtc2_h_sync_strt_wid = RREG32(RADEON_CRTC2_H_SYNC_STRT_WID);
+        crtc2_v_sync_strt_wid = RREG32(RADEON_CRTC2_V_SYNC_STRT_WID);
+
+        tmp = RREG32(RADEON_GPIO_MONID);
+        tmp &= ~RADEON_GPIO_A_0;
+        WREG32(RADEON_GPIO_MONID, tmp);
+
+        WREG32(RADEON_FP2_GEN_CNTL, (RADEON_FP2_ON |
+                                     RADEON_FP2_PANEL_FORMAT |
+                                     R200_FP2_SOURCE_SEL_TRANS_UNIT |
+                                     RADEON_FP2_DVO_EN |
+                                     R200_FP2_DVO_RATE_SEL_SDR));
+
+        WREG32(RADEON_DISP_OUTPUT_CNTL, (RADEON_DISP_DAC_SOURCE_RMX |
+                                         RADEON_DISP_TRANS_MATRIX_GRAPHICS));
+
+        WREG32(RADEON_CRTC2_GEN_CNTL, (RADEON_CRTC2_EN |
+                                       RADEON_CRTC2_DISP_REQ_EN_B));
+
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_A, 0x00000000);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_B, 0x000003f0);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_C, 0x00000000);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_D, 0x000003f0);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_E, 0x00000000);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_F, 0x000003f0);
+
+        WREG32(RADEON_CRTC2_H_TOTAL_DISP, 0x01000008);
+        WREG32(RADEON_CRTC2_H_SYNC_STRT_WID, 0x00000800);
+        WREG32(RADEON_CRTC2_V_TOTAL_DISP, 0x00080001);
+        WREG32(RADEON_CRTC2_V_SYNC_STRT_WID, 0x00000080);
+
+        for (i = 0; i < 200; i++) {
+                tmp = RREG32(RADEON_GPIO_MONID);
+                if (tmp & RADEON_GPIO_Y_0)
+                        found = true;
+
+                if (found)
+                        break;
+
+                if (!drm_can_sleep())
+                        mdelay(1);
+                else
+                        msleep(1);
+        }
+
+        /* restore the regs we used */
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_A, disp_lin_trans_grph_a);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_B, disp_lin_trans_grph_b);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_C, disp_lin_trans_grph_c);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_D, disp_lin_trans_grph_d);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_E, disp_lin_trans_grph_e);
+        WREG32(RADEON_DISP_LIN_TRANS_GRPH_F, disp_lin_trans_grph_f);
+        WREG32(RADEON_CRTC2_H_TOTAL_DISP, crtc2_h_total_disp);
+        WREG32(RADEON_CRTC2_V_TOTAL_DISP, crtc2_v_total_disp);
+        WREG32(RADEON_CRTC2_H_SYNC_STRT_WID, crtc2_h_sync_strt_wid);
+        WREG32(RADEON_CRTC2_V_SYNC_STRT_WID, crtc2_v_sync_strt_wid);
+        WREG32(RADEON_CRTC2_GEN_CNTL, crtc2_gen_cntl);
+        WREG32(RADEON_DISP_OUTPUT_CNTL, disp_output_cntl);
+        WREG32(RADEON_FP2_GEN_CNTL, fp2_gen_cntl);
+        WREG32(RADEON_GPIO_MONID, gpio_monid);
+
+        return found;
+}
+
 static enum drm_connector_status radeon_legacy_tv_dac_detect(struct drm_encoder *encoder,
                                                              struct drm_connector *connector)
 {
         struct drm_device *dev = encoder->dev;
         struct radeon_device *rdev = dev->dev_private;
-        uint32_t crtc2_gen_cntl, tv_dac_cntl, dac_cntl2, dac_ext_cntl;
-        uint32_t disp_hw_debug, disp_output_cntl, gpiopad_a, pixclks_cntl, tmp;
+        uint32_t crtc2_gen_cntl = 0, tv_dac_cntl, dac_cntl2, dac_ext_cntl;
+        uint32_t gpiopad_a = 0, pixclks_cntl, tmp;
+        uint32_t disp_output_cntl = 0, disp_hw_debug = 0, crtc_ext_cntl = 0;
         enum drm_connector_status found = connector_status_disconnected;
         struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
         struct radeon_encoder_tv_dac *tv_dac = radeon_encoder->enc_priv;
@@ -1460,12 +1556,27 @@ static enum drm_connector_status radeon_legacy_tv_dac_detect(struct drm_encoder
                 return connector_status_disconnected;
         }
 
+        /* R200 uses an external DAC for secondary DAC */
+        if (rdev->family == CHIP_R200) {
+                if (radeon_legacy_ext_dac_detect(encoder, connector))
+                        found = connector_status_connected;
+                return found;
+        }
+
         /* save the regs we need */
         pixclks_cntl = RREG32_PLL(RADEON_PIXCLKS_CNTL);
-        gpiopad_a = ASIC_IS_R300(rdev) ? RREG32(RADEON_GPIOPAD_A) : 0;
-        disp_output_cntl = ASIC_IS_R300(rdev) ? RREG32(RADEON_DISP_OUTPUT_CNTL) : 0;
-        disp_hw_debug = ASIC_IS_R300(rdev) ? 0 : RREG32(RADEON_DISP_HW_DEBUG);
-        crtc2_gen_cntl = RREG32(RADEON_CRTC2_GEN_CNTL);
+
+        if (rdev->flags & RADEON_SINGLE_CRTC) {
+                crtc_ext_cntl = RREG32(RADEON_CRTC_EXT_CNTL);
+        } else {
+                if (ASIC_IS_R300(rdev)) {
+                        gpiopad_a = RREG32(RADEON_GPIOPAD_A);
+                        disp_output_cntl = RREG32(RADEON_DISP_OUTPUT_CNTL);
+                } else {
+                        disp_hw_debug = RREG32(RADEON_DISP_HW_DEBUG);
+                }
+                crtc2_gen_cntl = RREG32(RADEON_CRTC2_GEN_CNTL);
+        }
         tv_dac_cntl = RREG32(RADEON_TV_DAC_CNTL);
         dac_ext_cntl = RREG32(RADEON_DAC_EXT_CNTL);
         dac_cntl2 = RREG32(RADEON_DAC_CNTL2);
@@ -1474,22 +1585,24 @@ static enum drm_connector_status radeon_legacy_tv_dac_detect(struct drm_encoder
                                | RADEON_PIX2CLK_DAC_ALWAYS_ONb);
         WREG32_PLL(RADEON_PIXCLKS_CNTL, tmp);
 
-        if (ASIC_IS_R300(rdev))
-                WREG32_P(RADEON_GPIOPAD_A, 1, ~1);
-
-        tmp = crtc2_gen_cntl & ~RADEON_CRTC2_PIX_WIDTH_MASK;
-        tmp |= RADEON_CRTC2_CRT2_ON |
-                (2 << RADEON_CRTC2_PIX_WIDTH_SHIFT);
-
-        WREG32(RADEON_CRTC2_GEN_CNTL, tmp);
-
-        if (ASIC_IS_R300(rdev)) {
-                tmp = disp_output_cntl & ~RADEON_DISP_TVDAC_SOURCE_MASK;
-                tmp |= RADEON_DISP_TVDAC_SOURCE_CRTC2;
-                WREG32(RADEON_DISP_OUTPUT_CNTL, tmp);
+        if (rdev->flags & RADEON_SINGLE_CRTC) {
+                tmp = crtc_ext_cntl | RADEON_CRTC_CRT_ON;
+                WREG32(RADEON_CRTC_EXT_CNTL, tmp);
         } else {
-                tmp = disp_hw_debug & ~RADEON_CRT2_DISP1_SEL;
-                WREG32(RADEON_DISP_HW_DEBUG, tmp);
+                tmp = crtc2_gen_cntl & ~RADEON_CRTC2_PIX_WIDTH_MASK;
+                tmp |= RADEON_CRTC2_CRT2_ON |
+                        (2 << RADEON_CRTC2_PIX_WIDTH_SHIFT);
+                WREG32(RADEON_CRTC2_GEN_CNTL, tmp);
+
+                if (ASIC_IS_R300(rdev)) {
+                        WREG32_P(RADEON_GPIOPAD_A, 1, ~1);
+                        tmp = disp_output_cntl & ~RADEON_DISP_TVDAC_SOURCE_MASK;
+                        tmp |= RADEON_DISP_TVDAC_SOURCE_CRTC2;
+                        WREG32(RADEON_DISP_OUTPUT_CNTL, tmp);
+                } else {
+                        tmp = disp_hw_debug & ~RADEON_CRT2_DISP1_SEL;
+                        WREG32(RADEON_DISP_HW_DEBUG, tmp);
+                }
         }
 
         tmp = RADEON_TV_DAC_NBLANK |
@@ -1531,14 +1644,19 @@ static enum drm_connector_status radeon_legacy_tv_dac_detect(struct drm_encoder
         WREG32(RADEON_DAC_CNTL2, dac_cntl2);
         WREG32(RADEON_DAC_EXT_CNTL, dac_ext_cntl);
         WREG32(RADEON_TV_DAC_CNTL, tv_dac_cntl);
-        WREG32(RADEON_CRTC2_GEN_CNTL, crtc2_gen_cntl);
 
-        if (ASIC_IS_R300(rdev)) {
-                WREG32(RADEON_DISP_OUTPUT_CNTL, disp_output_cntl);
-                WREG32_P(RADEON_GPIOPAD_A, gpiopad_a, ~1);
+        if (rdev->flags & RADEON_SINGLE_CRTC) {
+                WREG32(RADEON_CRTC_EXT_CNTL, crtc_ext_cntl);
         } else {
-                WREG32(RADEON_DISP_HW_DEBUG, disp_hw_debug);
+                WREG32(RADEON_CRTC2_GEN_CNTL, crtc2_gen_cntl);
+                if (ASIC_IS_R300(rdev)) {
+                        WREG32(RADEON_DISP_OUTPUT_CNTL, disp_output_cntl);
+                        WREG32_P(RADEON_GPIOPAD_A, gpiopad_a, ~1);
+                } else {
+                        WREG32(RADEON_DISP_HW_DEBUG, disp_hw_debug);
+                }
         }
+
         WREG32_PLL(RADEON_PIXCLKS_CNTL, pixclks_cntl);
 
         return found;
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 8b27dd6e3144..b91118ccef86 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -105,7 +105,6 @@ int radeon_bo_create(struct radeon_device *rdev,
         struct radeon_bo *bo;
         enum ttm_bo_type type;
         unsigned long page_align = roundup(byte_align, PAGE_SIZE) >> PAGE_SHIFT;
-        unsigned long max_size = 0;
         size_t acc_size;
         int r;
 
@@ -121,18 +120,9 @@ int radeon_bo_create(struct radeon_device *rdev,
         }
         *bo_ptr = NULL;
 
-        /* maximun bo size is the minimun btw visible vram and gtt size */
-        max_size = min(rdev->mc.visible_vram_size, rdev->mc.gtt_size);
-        if ((page_align << PAGE_SHIFT) >= max_size) {
-                printk(KERN_WARNING "%s:%d alloc size %ldM bigger than %ldMb limit\n",
-                        __func__, __LINE__, page_align  >> (20 - PAGE_SHIFT), max_size >> 20);
-                return -ENOMEM;
-        }
-
         acc_size = ttm_bo_dma_acc_size(&rdev->mman.bdev, size,
                                        sizeof(struct radeon_bo));
 
-retry:
         bo = kzalloc(sizeof(struct radeon_bo), GFP_KERNEL);
         if (bo == NULL)
                 return -ENOMEM;
@@ -154,15 +144,6 @@ retry:
                         acc_size, sg, &radeon_ttm_bo_destroy);
         up_read(&rdev->pm.mclk_lock);
         if (unlikely(r != 0)) {
-                if (r != -ERESTARTSYS) {
-                        if (domain == RADEON_GEM_DOMAIN_VRAM) {
-                                domain |= RADEON_GEM_DOMAIN_GTT;
-                                goto retry;
-                        }
-                        dev_err(rdev->dev,
-                                "object_init failed for (%lu, 0x%08X)\n",
-                                size, domain);
-                }
                 return r;
         }
         *bo_ptr = bo;
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index bba66902c83b..47634f27f2e5 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -305,7 +305,7 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 {
 #if DRM_DEBUG_CODE
         if (ring->count_dw <= 0) {
-                DRM_ERROR("radeon: writting more dword to ring than expected !\n");
+                DRM_ERROR("radeon: writing more dwords to the ring than expected!\n");
         }
 #endif
         ring->ring[ring->wptr++] = v;
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index f79633a036c3..b0db712060fb 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -2407,12 +2407,13 @@ static int si_pcie_gart_enable(struct radeon_device *rdev)
         WREG32(0x15DC, 0);
 
         /* empty context1-15 */
-        /* FIXME start with 4G, once using 2 level pt switch to full
-         * vm size space
-         */
         /* set vm size, must be a multiple of 4 */
         WREG32(VM_CONTEXT1_PAGE_TABLE_START_ADDR, 0);
         WREG32(VM_CONTEXT1_PAGE_TABLE_END_ADDR, rdev->vm_manager.max_pfn);
+        /* Assign the pt base to something valid for now; the pts used for
+         * the VMs are determined by the application and setup and assigned
+         * on the fly in the vm part of radeon_gart.c
+         */
         for (i = 1; i < 16; i++) {
                 if (i < 8)
                         WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2),
@@ -2807,26 +2808,31 @@ void si_vm_set_page(struct radeon_device *rdev, uint64_t pe,
 {
         struct radeon_ring *ring = &rdev->ring[rdev->asic->vm.pt_ring_index];
         uint32_t r600_flags = cayman_vm_page_flags(rdev, flags);
-        int i;
-        uint64_t value;
 
-        radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 2 + count * 2));
-        radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
-                                 WRITE_DATA_DST_SEL(1)));
-        radeon_ring_write(ring, pe);
-        radeon_ring_write(ring, upper_32_bits(pe));
-        for (i = 0; i < count; ++i) {
-                if (flags & RADEON_VM_PAGE_SYSTEM) {
-                        value = radeon_vm_map_gart(rdev, addr);
-                        value &= 0xFFFFFFFFFFFFF000ULL;
-                } else if (flags & RADEON_VM_PAGE_VALID)
-                        value = addr;
-                else
-                        value = 0;
-                addr += incr;
-                value |= r600_flags;
-                radeon_ring_write(ring, value);
-                radeon_ring_write(ring, upper_32_bits(value));
+        while (count) {
+                unsigned ndw = 2 + count * 2;
+                if (ndw > 0x3FFE)
+                        ndw = 0x3FFE;
+
+                radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, ndw));
+                radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+                                         WRITE_DATA_DST_SEL(1)));
+                radeon_ring_write(ring, pe);
+                radeon_ring_write(ring, upper_32_bits(pe));
+                for (; ndw > 2; ndw -= 2, --count, pe += 8) {
+                        uint64_t value;
+                        if (flags & RADEON_VM_PAGE_SYSTEM) {
+                                value = radeon_vm_map_gart(rdev, addr);
+                                value &= 0xFFFFFFFFFFFFF000ULL;
+                        } else if (flags & RADEON_VM_PAGE_VALID)
+                                value = addr;
+                        else
+                                value = 0;
+                        addr += incr;
+                        value |= r600_flags;
+                        radeon_ring_write(ring, value);
+                        radeon_ring_write(ring, upper_32_bits(value));
+                }
         }
 }
 
@@ -2867,6 +2873,10 @@ void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
         radeon_ring_write(ring, VM_INVALIDATE_REQUEST >> 2);
         radeon_ring_write(ring, 0);
         radeon_ring_write(ring, 1 << vm->id);
+
+        /* sync PFP to ME, otherwise we might get invalid PFP reads */
+        radeon_ring_write(ring, PACKET3(PACKET3_PFP_SYNC_ME, 0));
+        radeon_ring_write(ring, 0x0);
 }
 
 /*
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_drv.c b/drivers/gpu/drm/shmobile/shmob_drm_drv.c
index c71d493fd0c5..1c350fc4e449 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_drv.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_drv.c
@@ -201,6 +201,8 @@ static int shmob_drm_load(struct drm_device *dev, unsigned long flags)
                 goto done;
         }
 
+        platform_set_drvdata(pdev, sdev);
+
 done:
         if (ret)
                 shmob_drm_unload(dev);
@@ -299,11 +301,9 @@ static struct drm_driver shmob_drm_driver = {
 #if CONFIG_PM_SLEEP
 static int shmob_drm_pm_suspend(struct device *dev)
 {
-        struct platform_device *pdev = to_platform_device(dev);
-        struct drm_device *ddev = platform_get_drvdata(pdev);
-        struct shmob_drm_device *sdev = ddev->dev_private;
+        struct shmob_drm_device *sdev = dev_get_drvdata(dev);
 
-        drm_kms_helper_poll_disable(ddev);
+        drm_kms_helper_poll_disable(sdev->ddev);
         shmob_drm_crtc_suspend(&sdev->crtc);
 
         return 0;
@@ -311,9 +311,7 @@ static int shmob_drm_pm_suspend(struct device *dev)
 
 static int shmob_drm_pm_resume(struct device *dev)
 {
-        struct platform_device *pdev = to_platform_device(dev);
-        struct drm_device *ddev = platform_get_drvdata(pdev);
-        struct shmob_drm_device *sdev = ddev->dev_private;
+        struct shmob_drm_device *sdev = dev_get_drvdata(dev);
 
         mutex_lock(&sdev->ddev->mode_config.mutex);
         shmob_drm_crtc_resume(&sdev->crtc);
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index 402ab69f9f99..bf6e4b5a73b5 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -580,6 +580,7 @@ retry:
         if (unlikely(ret != 0))
                 return ret;
 
+retry_reserve:
         spin_lock(&glob->lru_lock);
 
         if (unlikely(list_empty(&bo->ddestroy))) {
@@ -587,14 +588,20 @@ retry:
                 return 0;
         }
 
-        ret = ttm_bo_reserve_locked(bo, interruptible,
-                                    no_wait_reserve, false, 0);
+        ret = ttm_bo_reserve_locked(bo, false, true, false, 0);
 
-        if (unlikely(ret != 0)) {
+        if (unlikely(ret == -EBUSY)) {
                 spin_unlock(&glob->lru_lock);
-                return ret;
+                if (likely(!no_wait_reserve))
+                        ret = ttm_bo_wait_unreserved(bo, interruptible);
+                if (unlikely(ret != 0))
+                        return ret;
+
+                goto retry_reserve;
         }
 
+        BUG_ON(ret != 0);
+
         /**
          * We can re-check for sync object without taking
          * the bo::lock since setting the sync object requires
@@ -811,17 +818,14 @@ retry:
                                           no_wait_reserve, no_wait_gpu);
                 kref_put(&bo->list_kref, ttm_bo_release_list);
 
-                if (likely(ret == 0 || ret == -ERESTARTSYS))
-                        return ret;
-
-                goto retry;
+                return ret;
         }
 
-        ret = ttm_bo_reserve_locked(bo, false, no_wait_reserve, false, 0);
+        ret = ttm_bo_reserve_locked(bo, false, true, false, 0);
 
         if (unlikely(ret == -EBUSY)) {
                 spin_unlock(&glob->lru_lock);
-                if (likely(!no_wait_gpu))
+                if (likely(!no_wait_reserve))
                         ret = ttm_bo_wait_unreserved(bo, interruptible);
 
                 kref_put(&bo->list_kref, ttm_bo_release_list);
diff --git a/drivers/gpu/drm/udl/udl_drv.h b/drivers/gpu/drm/udl/udl_drv.h
index fccd361f7b50..87aa5f5d3c88 100644
--- a/drivers/gpu/drm/udl/udl_drv.h
+++ b/drivers/gpu/drm/udl/udl_drv.h
@@ -104,7 +104,7 @@ udl_fb_user_fb_create(struct drm_device *dev,
 
 int udl_render_hline(struct drm_device *dev, int bpp, struct urb **urb_ptr,
                      const char *front, char **urb_buf_ptr,
-                     u32 byte_offset, u32 byte_width,
+                     u32 byte_offset, u32 device_byte_offset, u32 byte_width,
                      int *ident_ptr, int *sent_ptr);
 
 int udl_dumb_create(struct drm_file *file_priv,
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 69a2b16f42a6..d4ab3beaada0 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -114,9 +114,10 @@ static void udlfb_dpy_deferred_io(struct fb_info *info,
         list_for_each_entry(cur, &fbdefio->pagelist, lru) {
 
                 if (udl_render_hline(dev, (ufbdev->ufb.base.bits_per_pixel / 8),
-                                  &urb, (char *) info->fix.smem_start,
-                                  &cmd, cur->index << PAGE_SHIFT,
-                                  PAGE_SIZE, &bytes_identical, &bytes_sent))
+                                     &urb, (char *) info->fix.smem_start,
+                                     &cmd, cur->index << PAGE_SHIFT,
+                                     cur->index << PAGE_SHIFT,
+                                     PAGE_SIZE, &bytes_identical, &bytes_sent))
                         goto error;
                 bytes_rendered += PAGE_SIZE;
         }
@@ -187,10 +188,11 @@ int udl_handle_damage(struct udl_framebuffer *fb, int x, int y,
         for (i = y; i < y + height ; i++) {
                 const int line_offset = fb->base.pitches[0] * i;
                 const int byte_offset = line_offset + (x * bpp);
-
+                const int dev_byte_offset = (fb->base.width * bpp * i) + (x * bpp);
                 if (udl_render_hline(dev, bpp, &urb,
                                      (char *) fb->obj->vmapping,
-                                     &cmd, byte_offset, width * bpp,
+                                     &cmd, byte_offset, dev_byte_offset,
+                                     width * bpp,
                                      &bytes_identical, &bytes_sent))
                         goto error;
         }
diff --git a/drivers/gpu/drm/udl/udl_transfer.c b/drivers/gpu/drm/udl/udl_transfer.c
index dc095526ffb7..142fee5f983f 100644
--- a/drivers/gpu/drm/udl/udl_transfer.c
+++ b/drivers/gpu/drm/udl/udl_transfer.c
@@ -213,11 +213,12 @@ static void udl_compress_hline16(
  */
 int udl_render_hline(struct drm_device *dev, int bpp, struct urb **urb_ptr,
                      const char *front, char **urb_buf_ptr,
-                     u32 byte_offset, u32 byte_width,
+                     u32 byte_offset, u32 device_byte_offset,
+                     u32 byte_width,
                      int *ident_ptr, int *sent_ptr)
 {
         const u8 *line_start, *line_end, *next_pixel;
-        u32 base16 = 0 + (byte_offset / bpp) * 2;
+        u32 base16 = 0 + (device_byte_offset / bpp) * 2;
         struct urb *urb = *urb_ptr;
         u8 *cmd = *urb_buf_ptr;
         u8 *cmd_end = (u8 *) urb->transfer_buffer + urb->transfer_buffer_length;
diff --git a/drivers/hid/hid-apple.c b/drivers/hid/hid-apple.c
index 06ebdbb6ea02..fd7722aecf77 100644
--- a/drivers/hid/hid-apple.c
+++ b/drivers/hid/hid-apple.c
@@ -522,6 +522,12 @@ static const struct hid_device_id apple_devices[] = {
                 .driver_data = APPLE_HAS_FN | APPLE_ISO_KEYBOARD },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_JIS),
                 .driver_data = APPLE_HAS_FN | APPLE_RDESC_JIS },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI),
+                .driver_data = APPLE_HAS_FN },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO),
+                .driver_data = APPLE_HAS_FN | APPLE_ISO_KEYBOARD },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS),
+                .driver_data = APPLE_HAS_FN | APPLE_RDESC_JIS },
         { HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ANSI),
                 .driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN },
         { HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ISO),
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index bd3971bf31bf..f4109fd657ff 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -1532,6 +1532,9 @@ static const struct hid_device_id hid_have_special_driver[] = {
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_ANSI) },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_ISO) },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_JIS) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS) },
         { HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ANSI) },
         { HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ISO) },
         { HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_JIS) },
@@ -2139,6 +2142,9 @@ static const struct hid_device_id hid_mouse_ignore_list[] = {
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_ANSI) },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_ISO) },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7_JIS) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS) },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY) },
         { HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY) },
         { }
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 269b50912a4a..9d7a42857ea1 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -118,6 +118,9 @@
 #define USB_DEVICE_ID_APPLE_WELLSPRING5A_ANSI   0x0252
 #define USB_DEVICE_ID_APPLE_WELLSPRING5A_ISO    0x0253
 #define USB_DEVICE_ID_APPLE_WELLSPRING5A_JIS    0x0254
+#define USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI   0x0259
+#define USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO    0x025a
+#define USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS    0x025b
 #define USB_DEVICE_ID_APPLE_WELLSPRING6A_ANSI   0x0249
 #define USB_DEVICE_ID_APPLE_WELLSPRING6A_ISO    0x024a
 #define USB_DEVICE_ID_APPLE_WELLSPRING6A_JIS    0x024b
diff --git a/drivers/hid/hid-microsoft.c b/drivers/hid/hid-microsoft.c
index 3acdcfcc17df..f676c01bb471 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -28,22 +28,30 @@
 #define MS_RDESC                0x08
 #define MS_NOGET                0x10
 #define MS_DUPLICATE_USAGES     0x20
+#define MS_RDESC_3K             0x40
 
-/*
- * Microsoft Wireless Desktop Receiver (Model 1028) has
- * 'Usage Min/Max' where it ought to have 'Physical Min/Max'
- */
 static __u8 *ms_report_fixup(struct hid_device *hdev, __u8 *rdesc,
                 unsigned int *rsize)
 {
         unsigned long quirks = (unsigned long)hid_get_drvdata(hdev);
 
+        /*
+         * Microsoft Wireless Desktop Receiver (Model 1028) has
+         * 'Usage Min/Max' where it ought to have 'Physical Min/Max'
+         */
         if ((quirks & MS_RDESC) && *rsize == 571 && rdesc[557] == 0x19 &&
                         rdesc[559] == 0x29) {
                 hid_info(hdev, "fixing up Microsoft Wireless Receiver Model 1028 report descriptor\n");
                 rdesc[557] = 0x35;
                 rdesc[559] = 0x45;
         }
+        /* the same as above (s/usage/physical/) */
+        if ((quirks & MS_RDESC_3K) && *rsize == 106 &&
+                        !memcmp((char []){ 0x19, 0x00, 0x29, 0xff },
+                                &rdesc[94], 4)) {
+                rdesc[94] = 0x35;
+                rdesc[96] = 0x45;
+        }
         return rdesc;
 }
 
@@ -192,7 +200,7 @@ static const struct hid_device_id ms_devices[] = {
         { HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_USB),
                 .driver_data = MS_PRESENTER },
         { HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_3K),
-                .driver_data = MS_ERGONOMY },
+                .driver_data = MS_ERGONOMY | MS_RDESC_3K },
         { HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_WIRELESS_OPTICAL_DESKTOP_3_0),
                 .driver_data = MS_NOGET },
         { HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_MOUSE_4500),
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 3eb02b94fc87..7867d69f0efe 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -210,8 +210,7 @@ static struct mt_class mt_classes[] = {
         },
         { .name = MT_CLS_GENERALTOUCH_PWT_TENFINGERS,
                 .quirks = MT_QUIRK_NOT_SEEN_MEANS_UP |
-                        MT_QUIRK_SLOT_IS_CONTACTNUMBER,
-                .maxcontacts = 10
+                        MT_QUIRK_SLOT_IS_CONTACTNUMBER
         },
 
         { .name = MT_CLS_FLATFROG,
@@ -421,11 +420,11 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
                          * contact max are global to the report */
                         td->last_field_index = field->index;
                         return -1;
-                }
                 case HID_DG_TOUCH:
                         /* Legacy devices use TIPSWITCH and not TOUCH.
                          * Let's just ignore this field. */
                         return -1;
+                }
                 /* let hid-input decide for the others */
                 return 0;
 
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 406537420fff..f4c3d28cd1fc 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -146,14 +146,14 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 
         if (ret != 0) {
                 err = ret;
-                goto errorout;
+                goto error0;
         }
 
         ret = hv_ringbuffer_init(
                 &newchannel->inbound, in, recv_ringbuffer_size);
         if (ret != 0) {
                 err = ret;
-                goto errorout;
+                goto error0;
         }
 
 
@@ -168,7 +168,7 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 
         if (ret != 0) {
                 err = ret;
-                goto errorout;
+                goto error0;
         }
 
         /* Create and init the channel open message */
@@ -177,7 +177,7 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
                            GFP_KERNEL);
         if (!open_info) {
                 err = -ENOMEM;
-                goto errorout;
+                goto error0;
         }
 
         init_completion(&open_info->waitevent);
@@ -193,7 +193,7 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 
         if (userdatalen > MAX_USER_DEFINED_BYTES) {
                 err = -EINVAL;
-                goto errorout;
+                goto error0;
         }
 
         if (userdatalen)
@@ -208,19 +208,18 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
                                sizeof(struct vmbus_channel_open_channel));
 
         if (ret != 0)
-                goto cleanup;
+                goto error1;
 
         t = wait_for_completion_timeout(&open_info->waitevent, 5*HZ);
         if (t == 0) {
                 err = -ETIMEDOUT;
-                goto errorout;
+                goto error1;
         }
 
 
         if (open_info->response.open_result.status)
                 err = open_info->response.open_result.status;
 
-cleanup:
         spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
         list_del(&open_info->msglistentry);
         spin_unlock_irqrestore(&vmbus_connection.channelmsg_lock, flags);
@@ -228,9 +227,12 @@ cleanup:
         kfree(open_info);
         return err;
 
-errorout:
-        hv_ringbuffer_cleanup(&newchannel->outbound);
-        hv_ringbuffer_cleanup(&newchannel->inbound);
+error1:
+        spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
+        list_del(&open_info->msglistentry);
+        spin_unlock_irqrestore(&vmbus_connection.channelmsg_lock, flags);
+
+error0:
         free_pages((unsigned long)out,
                 get_order(send_ringbuffer_size + recv_ringbuffer_size));
         kfree(open_info);
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index c74e73b2069a..c4633de64465 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -334,16 +334,6 @@ config SENSORS_DA9052_ADC
           This driver can also be built as module. If so, the module
           will be called da9052-hwmon.
 
-config SENSORS_EXYNOS4_TMU
-        tristate "Temperature sensor on Samsung EXYNOS4"
-        depends on ARCH_EXYNOS4
-        help
-          If you say yes here you get support for TMU (Thermal Management
-          Unit) on SAMSUNG EXYNOS4 series of SoC.
-
-          This driver can also be built as a module. If so, the module
-          will be called exynos4-tmu.
-
 config SENSORS_I5K_AMB
         tristate "FB-DIMM AMB temperature sensor on Intel 5000 series chipsets"
         depends on PCI
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index a62ce17ddbfc..8d5fcb5e8e9f 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -50,7 +50,6 @@ obj-$(CONFIG_SENSORS_DS1621)	+= ds1621.o
 obj-$(CONFIG_SENSORS_EMC1403)   += emc1403.o
 obj-$(CONFIG_SENSORS_EMC2103)   += emc2103.o
 obj-$(CONFIG_SENSORS_EMC6W201)  += emc6w201.o
-obj-$(CONFIG_SENSORS_EXYNOS4_TMU)       += exynos4_tmu.o
 obj-$(CONFIG_SENSORS_F71805F)   += f71805f.o
 obj-$(CONFIG_SENSORS_F71882FG)  += f71882fg.o
 obj-$(CONFIG_SENSORS_F75375S)   += f75375s.o
diff --git a/drivers/hwmon/acpi_power_meter.c b/drivers/hwmon/acpi_power_meter.c
index 23ab3c496b05..1672e2a5db46 100644
--- a/drivers/hwmon/acpi_power_meter.c
+++ b/drivers/hwmon/acpi_power_meter.c
@@ -29,6 +29,7 @@
 #include <linux/kdev_t.h>
 #include <linux/sched.h>
 #include <linux/time.h>
+#include <linux/err.h>
 #include <acpi/acpi_drivers.h>
 #include <acpi/acpi_bus.h>
 
diff --git a/drivers/hwmon/adm9240.c b/drivers/hwmon/adm9240.c
index 8b24d1a4a2b4..dafa477715e3 100644
--- a/drivers/hwmon/adm9240.c
+++ b/drivers/hwmon/adm9240.c
@@ -50,6 +50,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
diff --git a/drivers/hwmon/adt7411.c b/drivers/hwmon/adt7411.c
index fe72c69a2d68..517f1856c706 100644
--- a/drivers/hwmon/adt7411.c
+++ b/drivers/hwmon/adt7411.c
@@ -15,7 +15,6 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/err.h>
-#include <linux/delay.h>
 #include <linux/mutex.h>
 #include <linux/jiffies.h>
 #include <linux/i2c.h>
diff --git a/drivers/hwmon/adt7462.c b/drivers/hwmon/adt7462.c
index baee482aedfc..98a7d81e25c5 100644
--- a/drivers/hwmon/adt7462.c
+++ b/drivers/hwmon/adt7462.c
@@ -26,7 +26,6 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
 #include <linux/log2.h>
 #include <linux/slab.h>
 
diff --git a/drivers/hwmon/adt7475.c b/drivers/hwmon/adt7475.c
index 861c756e9536..989e54c39252 100644
--- a/drivers/hwmon/adt7475.c
+++ b/drivers/hwmon/adt7475.c
@@ -20,6 +20,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
+#include <linux/jiffies.h>
 
 /* Indexes for the sysfs hooks */
 
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 8f3f6f2c45fd..b41baffa20f0 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -43,6 +43,7 @@
 #include <linux/leds.h>
 #include <linux/hwmon.h>
 #include <linux/workqueue.h>
+#include <linux/err.h>
 
 /* data port used by Apple SMC */
 #define APPLESMC_DATA_PORT      0x300
diff --git a/drivers/hwmon/asus_atk0110.c b/drivers/hwmon/asus_atk0110.c
index cccb0e9d45b4..56dbcfb3e301 100644
--- a/drivers/hwmon/asus_atk0110.c
+++ b/drivers/hwmon/asus_atk0110.c
@@ -14,6 +14,8 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/dmi.h>
+#include <linux/jiffies.h>
+#include <linux/err.h>
 
 #include <acpi/acpi.h>
 #include <acpi/acpixf.h>
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 984a3f13923b..47b8d84b489d 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -205,8 +205,11 @@ static const struct tjmax __cpuinitconst tjmax_table[] = {
         { "CPU N455", 100000 },
         { "CPU N470", 100000 },
         { "CPU N475", 100000 },
-        { "CPU  230", 100000 },
-        { "CPU  330", 125000 },
+        { "CPU  230", 100000 },         /* Model 0x1c, stepping 2       */
+        { "CPU  330", 125000 },         /* Model 0x1c, stepping 2       */
+        { "CPU CE4110", 110000 },       /* Model 0x1c, stepping 10      */
+        { "CPU CE4150", 110000 },       /* Model 0x1c, stepping 10      */
+        { "CPU CE4170", 110000 },       /* Model 0x1c, stepping 10      */
 };
 
 static int __cpuinit adjust_tjmax(struct cpuinfo_x86 *c, u32 id,
diff --git a/drivers/hwmon/da9052-hwmon.c b/drivers/hwmon/da9052-hwmon.c
index fc65f2d3ec91..b8d01c5f5713 100644
--- a/drivers/hwmon/da9052-hwmon.c
+++ b/drivers/hwmon/da9052-hwmon.c
@@ -12,7 +12,6 @@
  *
  */
 
-#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
diff --git a/drivers/hwmon/emc1403.c b/drivers/hwmon/emc1403.c
index 68ab94bde3f1..142e1cb8dea7 100644
--- a/drivers/hwmon/emc1403.c
+++ b/drivers/hwmon/emc1403.c
@@ -33,6 +33,7 @@
 #include <linux/err.h>
 #include <linux/sysfs.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 #define THERMAL_PID_REG         0xfd
 #define THERMAL_SMSC_ID_REG     0xfe
diff --git a/drivers/hwmon/emc6w201.c b/drivers/hwmon/emc6w201.c
index ada12a98a97c..a98c917b5888 100644
--- a/drivers/hwmon/emc6w201.c
+++ b/drivers/hwmon/emc6w201.c
@@ -18,7 +18,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
diff --git a/drivers/hwmon/exynos4_tmu.c b/drivers/hwmon/exynos4_tmu.c
deleted file mode 100644
index e912059140cd..000000000000
--- a/drivers/hwmon/exynos4_tmu.c
+++ /dev/null
@@ -1,518 +0,0 @@
-/*
- * exynos4_tmu.c - Samsung EXYNOS4 TMU (Thermal Management Unit)
- *
- *  Copyright (C) 2011 Samsung Electronics
- *  Donggeun Kim <dg77.kim@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- */
-
-#include <linux/module.h>
-#include <linux/err.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/clk.h>
-#include <linux/workqueue.h>
-#include <linux/sysfs.h>
-#include <linux/kobject.h>
-#include <linux/io.h>
-#include <linux/mutex.h>
-
-#include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
-
-#include <linux/platform_data/exynos4_tmu.h>
-
-#define EXYNOS4_TMU_REG_TRIMINFO        0x0
-#define EXYNOS4_TMU_REG_CONTROL         0x20
-#define EXYNOS4_TMU_REG_STATUS          0x28
-#define EXYNOS4_TMU_REG_CURRENT_TEMP    0x40
-#define EXYNOS4_TMU_REG_THRESHOLD_TEMP  0x44
-#define EXYNOS4_TMU_REG_TRIG_LEVEL0     0x50
-#define EXYNOS4_TMU_REG_TRIG_LEVEL1     0x54
-#define EXYNOS4_TMU_REG_TRIG_LEVEL2     0x58
-#define EXYNOS4_TMU_REG_TRIG_LEVEL3     0x5C
-#define EXYNOS4_TMU_REG_PAST_TEMP0      0x60
-#define EXYNOS4_TMU_REG_PAST_TEMP1      0x64
-#define EXYNOS4_TMU_REG_PAST_TEMP2      0x68
-#define EXYNOS4_TMU_REG_PAST_TEMP3      0x6C
-#define EXYNOS4_TMU_REG_INTEN           0x70
-#define EXYNOS4_TMU_REG_INTSTAT         0x74
-#define EXYNOS4_TMU_REG_INTCLEAR        0x78
-
-#define EXYNOS4_TMU_GAIN_SHIFT          8
-#define EXYNOS4_TMU_REF_VOLTAGE_SHIFT   24
-
-#define EXYNOS4_TMU_TRIM_TEMP_MASK      0xff
-#define EXYNOS4_TMU_CORE_ON     3
-#define EXYNOS4_TMU_CORE_OFF    2
-#define EXYNOS4_TMU_DEF_CODE_TO_TEMP_OFFSET     50
-#define EXYNOS4_TMU_TRIG_LEVEL0_MASK    0x1
-#define EXYNOS4_TMU_TRIG_LEVEL1_MASK    0x10
-#define EXYNOS4_TMU_TRIG_LEVEL2_MASK    0x100
-#define EXYNOS4_TMU_TRIG_LEVEL3_MASK    0x1000
-#define EXYNOS4_TMU_INTCLEAR_VAL        0x1111
-
-struct exynos4_tmu_data {
-        struct exynos4_tmu_platform_data *pdata;
-        struct device *hwmon_dev;
-        struct resource *mem;
-        void __iomem *base;
-        int irq;
-        struct work_struct irq_work;
-        struct mutex lock;
-        struct clk *clk;
-        u8 temp_error1, temp_error2;
-};
-
-/*
- * TMU treats temperature as a mapped temperature code.
- * The temperature is converted differently depending on the calibration type.
- */
-static int temp_to_code(struct exynos4_tmu_data *data, u8 temp)
-{
-        struct exynos4_tmu_platform_data *pdata = data->pdata;
-        int temp_code;
-
-        /* temp should range between 25 and 125 */
-        if (temp < 25 || temp > 125) {
-                temp_code = -EINVAL;
-                goto out;
-        }
-
-        switch (pdata->cal_type) {
-        case TYPE_TWO_POINT_TRIMMING:
-                temp_code = (temp - 25) *
-                    (data->temp_error2 - data->temp_error1) /
-                    (85 - 25) + data->temp_error1;
-                break;
-        case TYPE_ONE_POINT_TRIMMING:
-                temp_code = temp + data->temp_error1 - 25;
-                break;
-        default:
-                temp_code = temp + EXYNOS4_TMU_DEF_CODE_TO_TEMP_OFFSET;
-                break;
-        }
-out:
-        return temp_code;
-}
-
-/*
- * Calculate a temperature value from a temperature code.
- * The unit of the temperature is degree Celsius.
- */
-static int code_to_temp(struct exynos4_tmu_data *data, u8 temp_code)
-{
-        struct exynos4_tmu_platform_data *pdata = data->pdata;
-        int temp;
-
-        /* temp_code should range between 75 and 175 */
-        if (temp_code < 75 || temp_code > 175) {
-                temp = -ENODATA;
-                goto out;
-        }
-
-        switch (pdata->cal_type) {
-        case TYPE_TWO_POINT_TRIMMING:
-                temp = (temp_code - data->temp_error1) * (85 - 25) /
-                    (data->temp_error2 - data->temp_error1) + 25;
-                break;
-        case TYPE_ONE_POINT_TRIMMING:
-                temp = temp_code - data->temp_error1 + 25;
-                break;
-        default:
-                temp = temp_code - EXYNOS4_TMU_DEF_CODE_TO_TEMP_OFFSET;
-                break;
-        }
-out:
-        return temp;
-}
-
-static int exynos4_tmu_initialize(struct platform_device *pdev)
-{
-        struct exynos4_tmu_data *data = platform_get_drvdata(pdev);
-        struct exynos4_tmu_platform_data *pdata = data->pdata;
-        unsigned int status, trim_info;
-        int ret = 0, threshold_code;
-
-        mutex_lock(&data->lock);
-        clk_enable(data->clk);
-
-        status = readb(data->base + EXYNOS4_TMU_REG_STATUS);
-        if (!status) {
-                ret = -EBUSY;
-                goto out;
-        }
-
-        /* Save trimming info in order to perform calibration */
-        trim_info = readl(data->base + EXYNOS4_TMU_REG_TRIMINFO);
-        data->temp_error1 = trim_info & EXYNOS4_TMU_TRIM_TEMP_MASK;
-        data->temp_error2 = ((trim_info >> 8) & EXYNOS4_TMU_TRIM_TEMP_MASK);
-
-        /* Write temperature code for threshold */
-        threshold_code = temp_to_code(data, pdata->threshold);
-        if (threshold_code < 0) {
-                ret = threshold_code;
-                goto out;
-        }
-        writeb(threshold_code,
-                data->base + EXYNOS4_TMU_REG_THRESHOLD_TEMP);
-
-        writeb(pdata->trigger_levels[0],
-                data->base + EXYNOS4_TMU_REG_TRIG_LEVEL0);
-        writeb(pdata->trigger_levels[1],
-                data->base + EXYNOS4_TMU_REG_TRIG_LEVEL1);
-        writeb(pdata->trigger_levels[2],
-                data->base + EXYNOS4_TMU_REG_TRIG_LEVEL2);
-        writeb(pdata->trigger_levels[3],
-                data->base + EXYNOS4_TMU_REG_TRIG_LEVEL3);
-
-        writel(EXYNOS4_TMU_INTCLEAR_VAL,
-                data->base + EXYNOS4_TMU_REG_INTCLEAR);
-out:
-        clk_disable(data->clk);
-        mutex_unlock(&data->lock);
-
-        return ret;
-}
-
-static void exynos4_tmu_control(struct platform_device *pdev, bool on)
-{
-        struct exynos4_tmu_data *data = platform_get_drvdata(pdev);
-        struct exynos4_tmu_platform_data *pdata = data->pdata;
-        unsigned int con, interrupt_en;
-
-        mutex_lock(&data->lock);
-        clk_enable(data->clk);
-
-        con = pdata->reference_voltage << EXYNOS4_TMU_REF_VOLTAGE_SHIFT |
-                pdata->gain << EXYNOS4_TMU_GAIN_SHIFT;
-        if (on) {
-                con |= EXYNOS4_TMU_CORE_ON;
-                interrupt_en = pdata->trigger_level3_en << 12 |
-                        pdata->trigger_level2_en << 8 |
-                        pdata->trigger_level1_en << 4 |
-                        pdata->trigger_level0_en;
-        } else {
-                con |= EXYNOS4_TMU_CORE_OFF;
-                interrupt_en = 0; /* Disable all interrupts */
-        }
-        writel(interrupt_en, data->base + EXYNOS4_TMU_REG_INTEN);
-        writel(con, data->base + EXYNOS4_TMU_REG_CONTROL);
-
-        clk_disable(data->clk);
-        mutex_unlock(&data->lock);
-}
-
-static int exynos4_tmu_read(struct exynos4_tmu_data *data)
-{
-        u8 temp_code;
-        int temp;
-
-        mutex_lock(&data->lock);
-        clk_enable(data->clk);
-
-        temp_code = readb(data->base + EXYNOS4_TMU_REG_CURRENT_TEMP);
-        temp = code_to_temp(data, temp_code);
-
-        clk_disable(data->clk);
-        mutex_unlock(&data->lock);
-
-        return temp;
-}
-
-static void exynos4_tmu_work(struct work_struct *work)
-{
-        struct exynos4_tmu_data *data = container_of(work,
-                        struct exynos4_tmu_data, irq_work);
-
-        mutex_lock(&data->lock);
-        clk_enable(data->clk);
-
-        writel(EXYNOS4_TMU_INTCLEAR_VAL, data->base + EXYNOS4_TMU_REG_INTCLEAR);
-
-        kobject_uevent(&data->hwmon_dev->kobj, KOBJ_CHANGE);
-
-        enable_irq(data->irq);
-
-        clk_disable(data->clk);
-        mutex_unlock(&data->lock);
-}
-
-static irqreturn_t exynos4_tmu_irq(int irq, void *id)
-{
-        struct exynos4_tmu_data *data = id;
-
-        disable_irq_nosync(irq);
-        schedule_work(&data->irq_work);
-
-        return IRQ_HANDLED;
-}
-
-static ssize_t exynos4_tmu_show_name(struct device *dev,
-                struct device_attribute *attr, char *buf)
-{
-        return sprintf(buf, "exynos4-tmu\n");
-}
-
-static ssize_t exynos4_tmu_show_temp(struct device *dev,
-                struct device_attribute *attr, char *buf)
-{
-        struct exynos4_tmu_data *data = dev_get_drvdata(dev);
-        int ret;
-
-        ret = exynos4_tmu_read(data);
-        if (ret < 0)
-                return ret;
-
-        /* convert from degree Celsius to millidegree Celsius */
-        return sprintf(buf, "%d\n", ret * 1000);
-}
-
-static ssize_t exynos4_tmu_show_alarm(struct device *dev,
-                struct device_attribute *devattr, char *buf)
-{
-        struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-        struct exynos4_tmu_data *data = dev_get_drvdata(dev);
-        struct exynos4_tmu_platform_data *pdata = data->pdata;
-        int temp;
-        unsigned int trigger_level;
-
-        temp = exynos4_tmu_read(data);
-        if (temp < 0)
-                return temp;
-
-        trigger_level = pdata->threshold + pdata->trigger_levels[attr->index];
-
-        return sprintf(buf, "%d\n", !!(temp > trigger_level));
-}
-
-static ssize_t exynos4_tmu_show_level(struct device *dev,
-                struct device_attribute *devattr, char *buf)
-{
-        struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-        struct exynos4_tmu_data *data = dev_get_drvdata(dev);
-        struct exynos4_tmu_platform_data *pdata = data->pdata;
-        unsigned int temp = pdata->threshold +
-                        pdata->trigger_levels[attr->index];
-
-        return sprintf(buf, "%u\n", temp * 1000);
-}
-
-static DEVICE_ATTR(name, S_IRUGO, exynos4_tmu_show_name, NULL);
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, exynos4_tmu_show_temp, NULL, 0);
-
-static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO,
-                exynos4_tmu_show_alarm, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO,
-                exynos4_tmu_show_alarm, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp1_emergency_alarm, S_IRUGO,
-                exynos4_tmu_show_alarm, NULL, 3);
-
-static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, exynos4_tmu_show_level, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, exynos4_tmu_show_level, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp1_emergency, S_IRUGO,
-                exynos4_tmu_show_level, NULL, 3);
-
-static struct attribute *exynos4_tmu_attributes[] = {
-        &dev_attr_name.attr,
-        &sensor_dev_attr_temp1_input.dev_attr.attr,
-        &sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-        &sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-        &sensor_dev_attr_temp1_emergency_alarm.dev_attr.attr,
-        &sensor_dev_attr_temp1_max.dev_attr.attr,
-        &sensor_dev_attr_temp1_crit.dev_attr.attr,
-        &sensor_dev_attr_temp1_emergency.dev_attr.attr,
-        NULL,
-};
-
-static const struct attribute_group exynos4_tmu_attr_group = {
-        .attrs = exynos4_tmu_attributes,
-};
-
-static int __devinit exynos4_tmu_probe(struct platform_device *pdev)
-{
-        struct exynos4_tmu_data *data;
-        struct exynos4_tmu_platform_data *pdata = pdev->dev.platform_data;
-        int ret;
-
-        if (!pdata) {
-                dev_err(&pdev->dev, "No platform init data supplied.\n");
-                return -ENODEV;
-        }
-
-        data = kzalloc(sizeof(struct exynos4_tmu_data), GFP_KERNEL);
-        if (!data) {
-                dev_err(&pdev->dev, "Failed to allocate driver structure\n");
-                return -ENOMEM;
-        }
-
-        data->irq = platform_get_irq(pdev, 0);
-        if (data->irq < 0) {
-                ret = data->irq;
-                dev_err(&pdev->dev, "Failed to get platform irq\n");
-                goto err_free;
-        }
-
-        INIT_WORK(&data->irq_work, exynos4_tmu_work);
-
-        data->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!data->mem) {
-                ret = -ENOENT;
-                dev_err(&pdev->dev, "Failed to get platform resource\n");
-                goto err_free;
-        }
-
-        data->mem = request_mem_region(data->mem->start,
-                        resource_size(data->mem), pdev->name);
-        if (!data->mem) {
-                ret = -ENODEV;
-                dev_err(&pdev->dev, "Failed to request memory region\n");
-                goto err_free;
-        }
-
-        data->base = ioremap(data->mem->start, resource_size(data->mem));
-        if (!data->base) {
-                ret = -ENODEV;
-                dev_err(&pdev->dev, "Failed to ioremap memory\n");
-                goto err_mem_region;
-        }
-
-        ret = request_irq(data->irq, exynos4_tmu_irq,
-                IRQF_TRIGGER_RISING,
-                "exynos4-tmu", data);
-        if (ret) {
-                dev_err(&pdev->dev, "Failed to request irq: %d\n", data->irq);
-                goto err_io_remap;
-        }
-
-        data->clk = clk_get(NULL, "tmu_apbif");
-        if (IS_ERR(data->clk)) {
-                ret = PTR_ERR(data->clk);
-                dev_err(&pdev->dev, "Failed to get clock\n");
-                goto err_irq;
-        }
-
-        data->pdata = pdata;
-        platform_set_drvdata(pdev, data);
-        mutex_init(&data->lock);
-
-        ret = exynos4_tmu_initialize(pdev);
-        if (ret) {
-                dev_err(&pdev->dev, "Failed to initialize TMU\n");
-                goto err_clk;
-        }
-
-        ret = sysfs_create_group(&pdev->dev.kobj, &exynos4_tmu_attr_group);
-        if (ret) {
-                dev_err(&pdev->dev, "Failed to create sysfs group\n");
-                goto err_clk;
-        }
-
-        data->hwmon_dev = hwmon_device_register(&pdev->dev);
-        if (IS_ERR(data->hwmon_dev)) {
-                ret = PTR_ERR(data->hwmon_dev);
-                dev_err(&pdev->dev, "Failed to register hwmon device\n");
-                goto err_create_group;
-        }
-
-        exynos4_tmu_control(pdev, true);
-
-        return 0;
-
-err_create_group:
-        sysfs_remove_group(&pdev->dev.kobj, &exynos4_tmu_attr_group);
-err_clk:
-        platform_set_drvdata(pdev, NULL);
-        clk_put(data->clk);
-err_irq:
-        free_irq(data->irq, data);
-err_io_remap:
-        iounmap(data->base);
-err_mem_region:
-        release_mem_region(data->mem->start, resource_size(data->mem));
-err_free:
-        kfree(data);
-
-        return ret;
-}
-
-static int __devexit exynos4_tmu_remove(struct platform_device *pdev)
-{
-        struct exynos4_tmu_data *data = platform_get_drvdata(pdev);
-
-        exynos4_tmu_control(pdev, false);
-
-        hwmon_device_unregister(data->hwmon_dev);
-        sysfs_remove_group(&pdev->dev.kobj, &exynos4_tmu_attr_group);
-
-        clk_put(data->clk);
-
-        free_irq(data->irq, data);
-
-        iounmap(data->base);
-        release_mem_region(data->mem->start, resource_size(data->mem));
-
-        platform_set_drvdata(pdev, NULL);
-
-        kfree(data);
-
-        return 0;
-}
-
-#ifdef CONFIG_PM_SLEEP
-static int exynos4_tmu_suspend(struct device *dev)
-{
-        exynos4_tmu_control(to_platform_device(dev), false);
-
-        return 0;
-}
-
-static int exynos4_tmu_resume(struct device *dev)
-{
-        struct platform_device *pdev = to_platform_device(dev);
-
-        exynos4_tmu_initialize(pdev);
-        exynos4_tmu_control(pdev, true);
-
-        return 0;
-}
-
-static SIMPLE_DEV_PM_OPS(exynos4_tmu_pm,
-                         exynos4_tmu_suspend, exynos4_tmu_resume);
-#define EXYNOS4_TMU_PM  &exynos4_tmu_pm
-#else
-#define EXYNOS4_TMU_PM  NULL
-#endif
-
-static struct platform_driver exynos4_tmu_driver = {
-        .driver = {
-                .name   = "exynos4-tmu",
-                .owner  = THIS_MODULE,
-                .pm     = EXYNOS4_TMU_PM,
-        },
-        .probe = exynos4_tmu_probe,
-        .remove = __devexit_p(exynos4_tmu_remove),
-};
-
-module_platform_driver(exynos4_tmu_driver);
-
-MODULE_DESCRIPTION("EXYNOS4 TMU Driver");
-MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:exynos4-tmu");
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index 68ad7d255512..4f4110407387 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -2,7 +2,7 @@
  * fam15h_power.c - AMD Family 15h processor power monitoring
  *
  * Copyright (c) 2011 Advanced Micro Devices, Inc.
- * Author: Andreas Herrmann <andreas.herrmann3@amd.com>
+ * Author: Andreas Herrmann <herrmann.der.user@googlemail.com>
  *
  *
  * This driver is free software; you can redistribute it and/or
@@ -28,7 +28,7 @@
 #include <asm/processor.h>
 
 MODULE_DESCRIPTION("AMD Family 15h CPU processor power monitor");
-MODULE_AUTHOR("Andreas Herrmann <andreas.herrmann3@amd.com>");
+MODULE_AUTHOR("Andreas Herrmann <herrmann.der.user@googlemail.com>");
 MODULE_LICENSE("GPL");
 
 /* D18F3 */
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 36509ae32083..1381a2e3bbd4 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -630,7 +630,9 @@ static struct platform_driver gpio_fan_driver = {
         .driver = {
                 .name   = "gpio-fan",
                 .pm     = GPIO_FAN_PM,
+#ifdef CONFIG_OF_GPIO
                 .of_match_table = of_match_ptr(of_gpio_fan_match),
+#endif
         },
 };
 
diff --git a/drivers/hwmon/hih6130.c b/drivers/hwmon/hih6130.c
index e8ee75f55472..9a675efaa78d 100644
--- a/drivers/hwmon/hih6130.c
+++ b/drivers/hwmon/hih6130.c
@@ -33,6 +33,7 @@
 #include <linux/mutex.h>
 #include <linux/device.h>
 #include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /**
  * struct hih6130 - HIH-6130 device specific data
diff --git a/drivers/hwmon/i5k_amb.c b/drivers/hwmon/i5k_amb.c
index a18882cc073d..46141abaafba 100644
--- a/drivers/hwmon/i5k_amb.c
+++ b/drivers/hwmon/i5k_amb.c
@@ -21,12 +21,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/jiffies.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
 #include <linux/log2.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
diff --git a/drivers/hwmon/ibmaem.c b/drivers/hwmon/ibmaem.c
index 37f17e0d9d5d..a14f634248e7 100644
--- a/drivers/hwmon/ibmaem.c
+++ b/drivers/hwmon/ibmaem.c
@@ -36,6 +36,7 @@
 #include <linux/platform_device.h>
 #include <linux/math64.h>
 #include <linux/time.h>
+#include <linux/err.h>
 
 #define REFRESH_INTERVAL        (HZ)
 #define IPMI_TIMEOUT            (30 * HZ)
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 41dbf8161ed7..b622a93ec32c 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -26,6 +26,7 @@
 #include <linux/jiffies.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/err.h>
 
 #define REFRESH_INTERVAL        (2 * HZ)
 #define DRVNAME                 "ibmpex"
diff --git a/drivers/hwmon/ina2xx.c b/drivers/hwmon/ina2xx.c
index 70717d4a5e89..2b726346f8fa 100644
--- a/drivers/hwmon/ina2xx.c
+++ b/drivers/hwmon/ina2xx.c
@@ -33,6 +33,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 #include <linux/platform_data/ina2xx.h>
 
diff --git a/drivers/hwmon/k8temp.c b/drivers/hwmon/k8temp.c
index 49a69c5b3b8d..e8c7fb0bbf95 100644
--- a/drivers/hwmon/k8temp.c
+++ b/drivers/hwmon/k8temp.c
@@ -22,7 +22,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
diff --git a/drivers/hwmon/lineage-pem.c b/drivers/hwmon/lineage-pem.c
index bd75d2415432..41df29f59b0e 100644
--- a/drivers/hwmon/lineage-pem.c
+++ b/drivers/hwmon/lineage-pem.c
@@ -29,6 +29,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /*
  * This driver supports various Lineage Compact Power Line DC/DC and AC/DC
diff --git a/drivers/hwmon/lm92.c b/drivers/hwmon/lm92.c
index 2282d77e83e8..71626f3c8742 100644
--- a/drivers/hwmon/lm92.c
+++ b/drivers/hwmon/lm92.c
@@ -48,6 +48,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 /*
  * The LM92 and MAX6635 have 2 two-state pins for address selection,
diff --git a/drivers/hwmon/lm93.c b/drivers/hwmon/lm93.c
index c3d4255ed154..1a003f73e4e4 100644
--- a/drivers/hwmon/lm93.c
+++ b/drivers/hwmon/lm93.c
@@ -47,6 +47,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /* LM93 REGISTER ADDRESSES */
 
diff --git a/drivers/hwmon/ltc4151.c b/drivers/hwmon/ltc4151.c
index 8496baa08bc8..4319a94f549d 100644
--- a/drivers/hwmon/ltc4151.c
+++ b/drivers/hwmon/ltc4151.c
@@ -36,6 +36,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /* chip registers */
 #define LTC4151_SENSE_H 0x00
diff --git a/drivers/hwmon/ltc4215.c b/drivers/hwmon/ltc4215.c
index 98b3d04f98b7..e8876108a6b3 100644
--- a/drivers/hwmon/ltc4215.c
+++ b/drivers/hwmon/ltc4215.c
@@ -19,6 +19,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /* Here are names of the chip's registers (a.k.a. commands) */
 enum ltc4215_cmd {
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
index 52075914eb0b..3653f79dc2de 100644
--- a/drivers/hwmon/ltc4245.c
+++ b/drivers/hwmon/ltc4245.c
@@ -21,6 +21,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 #include <linux/i2c/ltc4245.h>
 
 /* Here are names of the chip's registers (a.k.a. commands) */
diff --git a/drivers/hwmon/ltc4261.c b/drivers/hwmon/ltc4261.c
index 77476a575c4e..84a2d2872b20 100644
--- a/drivers/hwmon/ltc4261.c
+++ b/drivers/hwmon/ltc4261.c
@@ -33,6 +33,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /* chip registers */
 #define LTC4261_STATUS  0x00    /* readonly */
diff --git a/drivers/hwmon/max16065.c b/drivers/hwmon/max16065.c
index 019427d7a5fd..e0019c69d1bb 100644
--- a/drivers/hwmon/max16065.c
+++ b/drivers/hwmon/max16065.c
@@ -22,7 +22,6 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
-#include <linux/delay.h>
 #include <linux/jiffies.h>
 
 enum chips { max16065, max16066, max16067, max16068, max16070, max16071 };
diff --git a/drivers/hwmon/max1619.c b/drivers/hwmon/max1619.c
index 6c11ec214071..445e5d40ac82 100644
--- a/drivers/hwmon/max1619.c
+++ b/drivers/hwmon/max1619.c
@@ -1,7 +1,7 @@
 /*
  * max1619.c - Part of lm_sensors, Linux kernel modules for hardware
  *             monitoring
- * Copyright (C) 2003-2004 Alexey Fisher <fishor@mail.ru>
+ * Copyright (C) 2003-2004 Oleksij Rempel <bug-track@fisher-privat.net>
  *                         Jean Delvare <khali@linux-fr.org>
  *
  * Based on the lm90 driver. The MAX1619 is a sensor chip made by Maxim.
@@ -357,7 +357,7 @@ static struct max1619_data *max1619_update_device(struct device *dev)
 
 module_i2c_driver(max1619_driver);
 
-MODULE_AUTHOR("Alexey Fisher <fishor@mail.ru> and "
+MODULE_AUTHOR("Oleksij Rempel <bug-track@fisher-privat.net> and "
         "Jean Delvare <khali@linux-fr.org>");
 MODULE_DESCRIPTION("MAX1619 sensor driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/max6642.c b/drivers/hwmon/max6642.c
index bf236c0782b7..223461a6d70f 100644
--- a/drivers/hwmon/max6642.c
+++ b/drivers/hwmon/max6642.c
@@ -7,7 +7,7 @@
  * Derived from:
  *
  *  Based on the max1619 driver.
- *  Copyright (C) 2003-2004 Alexey Fisher <fishor@mail.ru>
+ *  Copyright (C) 2003-2004 Oleksij Rempel <bug-track@fisher-privat.net>
  *                          Jean Delvare <khali@linux-fr.org>
  *
  * The MAX6642 is a sensor chip made by Maxim.
diff --git a/drivers/hwmon/pmbus/Kconfig b/drivers/hwmon/pmbus/Kconfig
index 2ca6a5a4f5a7..60745a535821 100644
--- a/drivers/hwmon/pmbus/Kconfig
+++ b/drivers/hwmon/pmbus/Kconfig
@@ -4,7 +4,7 @@
 
 menuconfig PMBUS
         tristate "PMBus support"
-        depends on I2C && EXPERIMENTAL
+        depends on I2C
         default n
         help
           Say yes here if you want to enable PMBus support.
diff --git a/drivers/hwmon/pmbus/pmbus_core.c b/drivers/hwmon/pmbus/pmbus_core.c
index 29b319db573e..7d19b1bb9ce6 100644
--- a/drivers/hwmon/pmbus/pmbus_core.c
+++ b/drivers/hwmon/pmbus/pmbus_core.c
@@ -26,7 +26,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
-#include <linux/delay.h>
+#include <linux/jiffies.h>
 #include <linux/i2c/pmbus.h>
 #include "pmbus.h"
 
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index fe11b95670bd..bcecd025fcc4 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -22,7 +22,6 @@
 
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/init.h>
 #include <linux/err.h>
diff --git a/drivers/hwmon/sht21.c b/drivers/hwmon/sht21.c
index c2565d04cd4a..5f67546950b1 100644
--- a/drivers/hwmon/sht21.c
+++ b/drivers/hwmon/sht21.c
@@ -29,6 +29,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/device.h>
+#include <linux/jiffies.h>
 
 /* I2C command bytes */
 #define SHT21_TRIG_T_MEASUREMENT_HM  0xe3
diff --git a/drivers/hwmon/smm665.c b/drivers/hwmon/smm665.c
index cbc51fb30dba..d9e1b7de78da 100644
--- a/drivers/hwmon/smm665.c
+++ b/drivers/hwmon/smm665.c
@@ -24,6 +24,7 @@
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /* Internal reference voltage (VREF, x 1000 */
 #define SMM665_VREF_ADC_X1000   1250
diff --git a/drivers/hwmon/thmc50.c b/drivers/hwmon/thmc50.c
index 080c26370480..3c2c48d904e6 100644
--- a/drivers/hwmon/thmc50.c
+++ b/drivers/hwmon/thmc50.c
@@ -28,6 +28,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 4e1ff82c63e0..b8777e54190a 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -26,6 +26,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/device.h>
+#include <linux/jiffies.h>
 
 #define DRIVER_NAME "tmp102"
 
diff --git a/drivers/hwmon/ultra45_env.c b/drivers/hwmon/ultra45_env.c
index c315c59f61fe..44136bb6d045 100644
--- a/drivers/hwmon/ultra45_env.c
+++ b/drivers/hwmon/ultra45_env.c
@@ -12,6 +12,7 @@
 #include <linux/io.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
 
 #define DRV_MODULE_VERSION      "0.1"
 
diff --git a/drivers/hwmon/w83791d.c b/drivers/hwmon/w83791d.c
index 93ea81a4bf35..39ab7bcc616e 100644
--- a/drivers/hwmon/w83791d.c
+++ b/drivers/hwmon/w83791d.c
@@ -41,6 +41,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 #define NUMBER_OF_VIN           10
 #define NUMBER_OF_FANIN         5
diff --git a/drivers/hwmon/w83792d.c b/drivers/hwmon/w83792d.c
index 06d6f56d4f69..053645279f38 100644
--- a/drivers/hwmon/w83792d.c
+++ b/drivers/hwmon/w83792d.c
@@ -44,6 +44,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
diff --git a/drivers/hwmon/w83793.c b/drivers/hwmon/w83793.c
index 4fc47e062071..99799fd1d917 100644
--- a/drivers/hwmon/w83793.c
+++ b/drivers/hwmon/w83793.c
@@ -46,6 +46,7 @@
 #include <linux/kref.h>
 #include <linux/notifier.h>
 #include <linux/reboot.h>
+#include <linux/jiffies.h>
 
 /* Default values */
 #define WATCHDOG_TIMEOUT 2      /* 2 minute default timeout */
diff --git a/drivers/hwmon/w83795.c b/drivers/hwmon/w83795.c
index b813c646c7ca..55a4f4894531 100644
--- a/drivers/hwmon/w83795.c
+++ b/drivers/hwmon/w83795.c
@@ -34,7 +34,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = {
diff --git a/drivers/hwmon/w83l786ng.c b/drivers/hwmon/w83l786ng.c
index c99c8a0473cf..f0e8286c3c70 100644
--- a/drivers/hwmon/w83l786ng.c
+++ b/drivers/hwmon/w83l786ng.c
@@ -33,6 +33,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2e, 0x2f, I2C_CLIENT_END };
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index beee6b2d361d..1722f50f2473 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_I2C_SMBUS)		+= i2c-smbus.o
 obj-$(CONFIG_I2C_CHARDEV)       += i2c-dev.o
 obj-$(CONFIG_I2C_MUX)           += i2c-mux.o
 obj-y                           += algos/ busses/ muxes/
+obj-$(CONFIG_I2C_STUB)          += i2c-stub.o
 
 ccflags-$(CONFIG_I2C_DEBUG_CORE) := -DDEBUG
 CFLAGS_i2c-core.o := -Wno-deprecated-declarations
diff --git a/drivers/i2c/algos/i2c-algo-pca.c b/drivers/i2c/algos/i2c-algo-pca.c
index 6f5f98d69af7..f892a424009b 100644
--- a/drivers/i2c/algos/i2c-algo-pca.c
+++ b/drivers/i2c/algos/i2c-algo-pca.c
@@ -46,14 +46,19 @@ static int i2c_debug;
 #define pca_set_con(adap, val) pca_outw(adap, I2C_PCA_CON, val)
 #define pca_get_con(adap) pca_inw(adap, I2C_PCA_CON)
 #define pca_wait(adap) adap->wait_for_completion(adap->data)
-#define pca_reset(adap) adap->reset_chip(adap->data)
 
-static void pca9665_reset(void *pd)
+static void pca_reset(struct i2c_algo_pca_data *adap)
 {
-        struct i2c_algo_pca_data *adap = pd;
-        pca_outw(adap, I2C_PCA_INDPTR, I2C_PCA_IPRESET);
-        pca_outw(adap, I2C_PCA_IND, 0xA5);
-        pca_outw(adap, I2C_PCA_IND, 0x5A);
+        if (adap->chip == I2C_PCA_CHIP_9665) {
+                /* Ignore the reset function from the module,
+                 * we can use the parallel bus reset.
+                 */
+                pca_outw(adap, I2C_PCA_INDPTR, I2C_PCA_IPRESET);
+                pca_outw(adap, I2C_PCA_IND, 0xA5);
+                pca_outw(adap, I2C_PCA_IND, 0x5A);
+        } else {
+                adap->reset_chip(adap->data);
+        }
 }
 
 /*
@@ -378,11 +383,12 @@ static unsigned int pca_probe_chip(struct i2c_adapter *adap)
         pca_outw(pca_data, I2C_PCA_INDPTR, I2C_PCA_IADR);
         if (pca_inw(pca_data, I2C_PCA_IND) == 0xAA) {
                 printk(KERN_INFO "%s: PCA9665 detected.\n", adap->name);
-                return I2C_PCA_CHIP_9665;
+                pca_data->chip = I2C_PCA_CHIP_9665;
         } else {
                 printk(KERN_INFO "%s: PCA9564 detected.\n", adap->name);
-                return I2C_PCA_CHIP_9564;
+                pca_data->chip = I2C_PCA_CHIP_9564;
         }
+        return pca_data->chip;
 }
 
 static int pca_init(struct i2c_adapter *adap)
@@ -456,11 +462,6 @@ static int pca_init(struct i2c_adapter *adap)
                  */
                 int raise_fall_time;
 
-                /* Ignore the reset function from the module,
-                 * we can use the parallel bus reset
-                 */
-                pca_data->reset_chip = pca9665_reset;
-
                 if (pca_data->i2c_clock > 1265800) {
                         printk(KERN_WARNING "%s: I2C clock speed too high."
                                 " Using 1265.8kHz.\n", adap->name);
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index ff01c389e2da..e9df4612b7eb 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -81,7 +81,6 @@ config I2C_I801
         tristate "Intel 82801 (ICH/PCH)"
         depends on PCI
         select CHECK_SIGNATURE if X86 && DMI
-        select GPIOLIB if I2C_MUX
         help
           If you say yes to this option, support will be included for the Intel
           801 family of mainboard I2C interfaces.  Specifically, the following
@@ -294,18 +293,21 @@ comment "I2C system bus drivers (mostly embedded / system-on-chip)"
 
 config I2C_AT91
         tristate "Atmel AT91 I2C Two-Wire interface (TWI)"
-        depends on ARCH_AT91 && EXPERIMENTAL && BROKEN
+        depends on ARCH_AT91 && EXPERIMENTAL
         help
           This supports the use of the I2C interface on Atmel AT91
           processors.
 
-          This driver is BROKEN because the controller which it uses
-          will easily trigger RX overrun and TX underrun errors.  Using
-          low I2C clock rates may partially work around those issues
-          on some systems.  Another serious problem is that there is no
-          documented way to issue repeated START conditions, as needed
+          A serious problem is that there is no documented way to issue
+          repeated START conditions for more than two messages, as needed
           to support combined I2C messages.  Use the i2c-gpio driver
-          unless your system can cope with those limitations.
+          unless your system can cope with this limitation.
+
+          Caution! at91rm9200, at91sam9261, at91sam9260, at91sam9263 devices
+          don't have clock stretching in transmission mode. For that reason,
+          you can encounter underrun issues causing premature stop sendings if
+          the latency to fill the transmission register is too long. If you
+          are facing this situation, use the i2c-gpio driver.
 
 config I2C_AU1550
         tristate "Au1550/Au1200/Au1300 SMBus interface"
@@ -718,6 +720,16 @@ config I2C_XLR
           This driver can also be built as a module.  If so, the module
           will be called i2c-xlr.
 
+config I2C_RCAR
+        tristate "Renesas R-Car I2C Controller"
+        depends on ARCH_SHMOBILE && I2C
+        help
+          If you say yes to this option, support will be included for the
+          R-Car I2C controller.
+
+          This driver can also be built as a module.  If so, the module
+          will be called i2c-rcar.
+
 comment "External I2C/SMBus adapter drivers"
 
 config I2C_DIOLAN_U2C
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 37c4182cc98b..395b516ffa08 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -71,6 +71,7 @@ obj-$(CONFIG_I2C_VERSATILE)	+= i2c-versatile.o
 obj-$(CONFIG_I2C_OCTEON)        += i2c-octeon.o
 obj-$(CONFIG_I2C_XILINX)        += i2c-xiic.o
 obj-$(CONFIG_I2C_XLR)           += i2c-xlr.o
+obj-$(CONFIG_I2C_RCAR)          += i2c-rcar.o
 
 # External I2C/SMBus adapter drivers
 obj-$(CONFIG_I2C_DIOLAN_U2C)    += i2c-diolan-u2c.o
@@ -84,7 +85,6 @@ obj-$(CONFIG_I2C_ACORN)		+= i2c-acorn.o
 obj-$(CONFIG_I2C_ELEKTOR)       += i2c-elektor.o
 obj-$(CONFIG_I2C_PCA_ISA)       += i2c-pca-isa.o
 obj-$(CONFIG_I2C_SIBYTE)        += i2c-sibyte.o
-obj-$(CONFIG_I2C_STUB)          += i2c-stub.o
 obj-$(CONFIG_SCx200_ACB)        += scx200_acb.o
 obj-$(CONFIG_SCx200_I2C)        += scx200_i2c.o
 
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index e24484beef07..aa59a254be2c 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -1,315 +1,554 @@
 /*
-    i2c Support for Atmel's AT91 Two-Wire Interface (TWI)
-
-    Copyright (C) 2004 Rick Bronson
-    Converted to 2.6 by Andrew Victor <andrew@sanpeople.com>
-
-    Borrowed heavily from original work by:
-    Copyright (C) 2000 Philip Edelbrock <phil@stimpy.netroedge.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.
-*/
+ *  i2c Support for Atmel's AT91 Two-Wire Interface (TWI)
+ *
+ *  Copyright (C) 2011 Weinmann Medical GmbH
+ *  Author: Nikolaus Voss <n.voss@weinmann.de>
+ *
+ *  Evolved from original work by:
+ *  Copyright (C) 2004 Rick Bronson
+ *  Converted to 2.6 by Andrew Victor <andrew@sanpeople.com>
+ *
+ *  Borrowed heavily from original work by:
+ *  Copyright (C) 2000 Philip Edelbrock <phil@stimpy.netroedge.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ */
 
-#include <linux/module.h>
-#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/err.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/delay.h>
 #include <linux/i2c.h>
-#include <linux/init.h>
-#include <linux/clk.h>
-#include <linux/platform_device.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_i2c.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define TWI_CLK_HZ              100000                  /* max 400 Kbits/s */
+#define AT91_I2C_TIMEOUT        msecs_to_jiffies(100)   /* transfer timeout */
+
+/* AT91 TWI register definitions */
+#define AT91_TWI_CR             0x0000  /* Control Register */
+#define AT91_TWI_START          0x0001  /* Send a Start Condition */
+#define AT91_TWI_STOP           0x0002  /* Send a Stop Condition */
+#define AT91_TWI_MSEN           0x0004  /* Master Transfer Enable */
+#define AT91_TWI_SVDIS          0x0020  /* Slave Transfer Disable */
+#define AT91_TWI_SWRST          0x0080  /* Software Reset */
+
+#define AT91_TWI_MMR            0x0004  /* Master Mode Register */
+#define AT91_TWI_IADRSZ_1       0x0100  /* Internal Device Address Size */
+#define AT91_TWI_MREAD          0x1000  /* Master Read Direction */
+
+#define AT91_TWI_IADR           0x000c  /* Internal Address Register */
+
+#define AT91_TWI_CWGR           0x0010  /* Clock Waveform Generator Reg */
+
+#define AT91_TWI_SR             0x0020  /* Status Register */
+#define AT91_TWI_TXCOMP         0x0001  /* Transmission Complete */
+#define AT91_TWI_RXRDY          0x0002  /* Receive Holding Register Ready */
+#define AT91_TWI_TXRDY          0x0004  /* Transmit Holding Register Ready */
 
-#include <mach/at91_twi.h>
-#include <mach/board.h>
-#include <mach/cpu.h>
+#define AT91_TWI_OVRE           0x0040  /* Overrun Error */
+#define AT91_TWI_UNRE           0x0080  /* Underrun Error */
+#define AT91_TWI_NACK           0x0100  /* Not Acknowledged */
 
-#define TWI_CLOCK               100000          /* Hz. max 400 Kbits/sec */
+#define AT91_TWI_IER            0x0024  /* Interrupt Enable Register */
+#define AT91_TWI_IDR            0x0028  /* Interrupt Disable Register */
+#define AT91_TWI_IMR            0x002c  /* Interrupt Mask Register */
+#define AT91_TWI_RHR            0x0030  /* Receive Holding Register */
+#define AT91_TWI_THR            0x0034  /* Transmit Holding Register */
 
+struct at91_twi_pdata {
+        unsigned        clk_max_div;
+        unsigned        clk_offset;
+        bool            has_unre_flag;
+};
+
+struct at91_twi_dev {
+        struct device           *dev;
+        void __iomem            *base;
+        struct completion       cmd_complete;
+        struct clk              *clk;
+        u8                      *buf;
+        size_t                  buf_len;
+        struct i2c_msg          *msg;
+        int                     irq;
+        unsigned                transfer_status;
+        struct i2c_adapter      adapter;
+        unsigned                twi_cwgr_reg;
+        struct at91_twi_pdata   *pdata;
+};
 
-static struct clk *twi_clk;
-static void __iomem *twi_base;
+static unsigned at91_twi_read(struct at91_twi_dev *dev, unsigned reg)
+{
+        return readl_relaxed(dev->base + reg);
+}
+
+static void at91_twi_write(struct at91_twi_dev *dev, unsigned reg, unsigned val)
+{
+        writel_relaxed(val, dev->base + reg);
+}
 
-#define at91_twi_read(reg)              __raw_readl(twi_base + (reg))
-#define at91_twi_write(reg, val)        __raw_writel((val), twi_base + (reg))
+static void at91_disable_twi_interrupts(struct at91_twi_dev *dev)
+{
+        at91_twi_write(dev, AT91_TWI_IDR,
+                       AT91_TWI_TXCOMP | AT91_TWI_RXRDY | AT91_TWI_TXRDY);
+}
 
+static void at91_init_twi_bus(struct at91_twi_dev *dev)
+{
+        at91_disable_twi_interrupts(dev);
+        at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SWRST);
+        at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSEN);
+        at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SVDIS);
+        at91_twi_write(dev, AT91_TWI_CWGR, dev->twi_cwgr_reg);
+}
 
 /*
- * Initialize the TWI hardware registers.
+ * Calculate symmetric clock as stated in datasheet:
+ * twi_clk = F_MAIN / (2 * (cdiv * (1 << ckdiv) + offset))
  */
-static void __devinit at91_twi_hwinit(void)
+static void __devinit at91_calc_twi_clock(struct at91_twi_dev *dev, int twi_clk)
 {
-        unsigned long cdiv, ckdiv;
-
-        at91_twi_write(AT91_TWI_IDR, 0xffffffff);       /* Disable all interrupts */
-        at91_twi_write(AT91_TWI_CR, AT91_TWI_SWRST);    /* Reset peripheral */
-        at91_twi_write(AT91_TWI_CR, AT91_TWI_MSEN);     /* Set Master mode */
-
-        /* Calcuate clock dividers */
-        cdiv = (clk_get_rate(twi_clk) / (2 * TWI_CLOCK)) - 3;
-        cdiv = cdiv + 1;        /* round up */
-        ckdiv = 0;
-        while (cdiv > 255) {
-                ckdiv++;
-                cdiv = cdiv >> 1;
+        int ckdiv, cdiv, div;
+        struct at91_twi_pdata *pdata = dev->pdata;
+        int offset = pdata->clk_offset;
+        int max_ckdiv = pdata->clk_max_div;
+
+        div = max(0, (int)DIV_ROUND_UP(clk_get_rate(dev->clk),
+                                       2 * twi_clk) - offset);
+        ckdiv = fls(div >> 8);
+        cdiv = div >> ckdiv;
+
+        if (ckdiv > max_ckdiv) {
+                dev_warn(dev->dev, "%d exceeds ckdiv max value which is %d.\n",
+                         ckdiv, max_ckdiv);
+                ckdiv = max_ckdiv;
+                cdiv = 255;
         }
 
-        if (cpu_is_at91rm9200()) {                      /* AT91RM9200 Errata #22 */
-                if (ckdiv > 5) {
-                        printk(KERN_ERR "AT91 I2C: Invalid TWI_CLOCK value!\n");
-                        ckdiv = 5;
-                }
-        }
+        dev->twi_cwgr_reg = (ckdiv << 16) | (cdiv << 8) | cdiv;
+        dev_dbg(dev->dev, "cdiv %d ckdiv %d\n", cdiv, ckdiv);
+}
 
-        at91_twi_write(AT91_TWI_CWGR, (ckdiv << 16) | (cdiv << 8) | cdiv);
+static void at91_twi_write_next_byte(struct at91_twi_dev *dev)
+{
+        if (dev->buf_len <= 0)
+                return;
+
+        at91_twi_write(dev, AT91_TWI_THR, *dev->buf);
+
+        /* send stop when last byte has been written */
+        if (--dev->buf_len == 0)
+                at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
+
+        dev_dbg(dev->dev, "wrote 0x%x, to go %d\n", *dev->buf, dev->buf_len);
+
+        ++dev->buf;
 }
 
-/*
- * Poll the i2c status register until the specified bit is set.
- * Returns 0 if timed out (100 msec).
- */
-static short at91_poll_status(unsigned long bit)
+static void at91_twi_read_next_byte(struct at91_twi_dev *dev)
 {
-        int loop_cntr = 10000;
+        if (dev->buf_len <= 0)
+                return;
+
+        *dev->buf = at91_twi_read(dev, AT91_TWI_RHR) & 0xff;
+        --dev->buf_len;
+
+        /* handle I2C_SMBUS_BLOCK_DATA */
+        if (unlikely(dev->msg->flags & I2C_M_RECV_LEN)) {
+                dev->msg->flags &= ~I2C_M_RECV_LEN;
+                dev->buf_len += *dev->buf;
+                dev->msg->len = dev->buf_len + 1;
+                dev_dbg(dev->dev, "received block length %d\n", dev->buf_len);
+        }
+
+        /* send stop if second but last byte has been read */
+        if (dev->buf_len == 1)
+                at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
 
-        do {
-                udelay(10);
-        } while (!(at91_twi_read(AT91_TWI_SR) & bit) && (--loop_cntr > 0));
+        dev_dbg(dev->dev, "read 0x%x, to go %d\n", *dev->buf, dev->buf_len);
 
-        return (loop_cntr > 0);
+        ++dev->buf;
 }
 
-static int xfer_read(struct i2c_adapter *adap, unsigned char *buf, int length)
+static irqreturn_t atmel_twi_interrupt(int irq, void *dev_id)
 {
-        /* Send Start */
-        at91_twi_write(AT91_TWI_CR, AT91_TWI_START);
-
-        /* Read data */
-        while (length--) {
-                if (!length)    /* need to send Stop before reading last byte */
-                        at91_twi_write(AT91_TWI_CR, AT91_TWI_STOP);
-                if (!at91_poll_status(AT91_TWI_RXRDY)) {
-                        dev_dbg(&adap->dev, "RXRDY timeout\n");
-                        return -ETIMEDOUT;
-                }
-                *buf++ = (at91_twi_read(AT91_TWI_RHR) & 0xff);
+        struct at91_twi_dev *dev = dev_id;
+        const unsigned status = at91_twi_read(dev, AT91_TWI_SR);
+        const unsigned irqstatus = status & at91_twi_read(dev, AT91_TWI_IMR);
+
+        if (!irqstatus)
+                return IRQ_NONE;
+        else if (irqstatus & AT91_TWI_RXRDY)
+                at91_twi_read_next_byte(dev);
+        else if (irqstatus & AT91_TWI_TXRDY)
+                at91_twi_write_next_byte(dev);
+
+        /* catch error flags */
+        dev->transfer_status |= status;
+
+        if (irqstatus & AT91_TWI_TXCOMP) {
+                at91_disable_twi_interrupts(dev);
+                complete(&dev->cmd_complete);
         }
 
-        return 0;
+        return IRQ_HANDLED;
 }
 
-static int xfer_write(struct i2c_adapter *adap, unsigned char *buf, int length)
+static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 {
-        /* Load first byte into transmitter */
-        at91_twi_write(AT91_TWI_THR, *buf++);
+        int ret;
+        bool has_unre_flag = dev->pdata->has_unre_flag;
 
-        /* Send Start */
-        at91_twi_write(AT91_TWI_CR, AT91_TWI_START);
+        dev_dbg(dev->dev, "transfer: %s %d bytes.\n",
+                (dev->msg->flags & I2C_M_RD) ? "read" : "write", dev->buf_len);
 
-        do {
-                if (!at91_poll_status(AT91_TWI_TXRDY)) {
-                        dev_dbg(&adap->dev, "TXRDY timeout\n");
-                        return -ETIMEDOUT;
-                }
+        INIT_COMPLETION(dev->cmd_complete);
+        dev->transfer_status = 0;
+        if (dev->msg->flags & I2C_M_RD) {
+                unsigned start_flags = AT91_TWI_START;
 
-                length--;       /* byte was transmitted */
+                if (at91_twi_read(dev, AT91_TWI_SR) & AT91_TWI_RXRDY) {
+                        dev_err(dev->dev, "RXRDY still set!");
+                        at91_twi_read(dev, AT91_TWI_RHR);
+                }
 
-                if (length > 0)         /* more data to send? */
-                        at91_twi_write(AT91_TWI_THR, *buf++);
-        } while (length);
+                /* if only one byte is to be read, immediately stop transfer */
+                if (dev->buf_len <= 1 && !(dev->msg->flags & I2C_M_RECV_LEN))
+                        start_flags |= AT91_TWI_STOP;
+                at91_twi_write(dev, AT91_TWI_CR, start_flags);
+                at91_twi_write(dev, AT91_TWI_IER,
+                               AT91_TWI_TXCOMP | AT91_TWI_RXRDY);
+        } else {
+                at91_twi_write_next_byte(dev);
+                at91_twi_write(dev, AT91_TWI_IER,
+                               AT91_TWI_TXCOMP | AT91_TWI_TXRDY);
+        }
 
-        /* Send Stop */
-        at91_twi_write(AT91_TWI_CR, AT91_TWI_STOP);
+        ret = wait_for_completion_interruptible_timeout(&dev->cmd_complete,
+                                                        dev->adapter.timeout);
+        if (ret == 0) {
+                dev_err(dev->dev, "controller timed out\n");
+                at91_init_twi_bus(dev);
+                return -ETIMEDOUT;
+        }
+        if (dev->transfer_status & AT91_TWI_NACK) {
+                dev_dbg(dev->dev, "received nack\n");
+                return -EREMOTEIO;
+        }
+        if (dev->transfer_status & AT91_TWI_OVRE) {
+                dev_err(dev->dev, "overrun while reading\n");
+                return -EIO;
+        }
+        if (has_unre_flag && dev->transfer_status & AT91_TWI_UNRE) {
+                dev_err(dev->dev, "underrun while writing\n");
+                return -EIO;
+        }
+        dev_dbg(dev->dev, "transfer complete\n");
 
         return 0;
 }
 
-/*
- * Generic i2c master transfer entrypoint.
- *
- * Note: We do not use Atmel's feature of storing the "internal device address".
- * Instead the "internal device address" has to be written using a separate
- * i2c message.
- * http://lists.arm.linux.org.uk/pipermail/linux-arm-kernel/2004-September/024411.html
- */
-static int at91_xfer(struct i2c_adapter *adap, struct i2c_msg *pmsg, int num)
+static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 {
-        int i, ret;
+        struct at91_twi_dev *dev = i2c_get_adapdata(adap);
+        int ret;
+        unsigned int_addr_flag = 0;
+        struct i2c_msg *m_start = msg;
 
         dev_dbg(&adap->dev, "at91_xfer: processing %d messages:\n", num);
 
-        for (i = 0; i < num; i++) {
-                dev_dbg(&adap->dev, " #%d: %sing %d byte%s %s 0x%02x\n", i,
-                        pmsg->flags & I2C_M_RD ? "read" : "writ",
-                        pmsg->len, pmsg->len > 1 ? "s" : "",
-                        pmsg->flags & I2C_M_RD ? "from" : "to", pmsg->addr);
-
-                at91_twi_write(AT91_TWI_MMR, (pmsg->addr << 16)
-                        | ((pmsg->flags & I2C_M_RD) ? AT91_TWI_MREAD : 0));
-
-                if (pmsg->len && pmsg->buf) {   /* sanity check */
-                        if (pmsg->flags & I2C_M_RD)
-                                ret = xfer_read(adap, pmsg->buf, pmsg->len);
-                        else
-                                ret = xfer_write(adap, pmsg->buf, pmsg->len);
-
-                        if (ret)
-                                return ret;
-
-                        /* Wait until transfer is finished */
-                        if (!at91_poll_status(AT91_TWI_TXCOMP)) {
-                                dev_dbg(&adap->dev, "TXCOMP timeout\n");
-                                return -ETIMEDOUT;
-                        }
+        /*
+         * The hardware can handle at most two messages concatenated by a
+         * repeated start via it's internal address feature.
+         */
+        if (num > 2) {
+                dev_err(dev->dev,
+                        "cannot handle more than two concatenated messages.\n");
+                return 0;
+        } else if (num == 2) {
+                int internal_address = 0;
+                int i;
+
+                if (msg->flags & I2C_M_RD) {
+                        dev_err(dev->dev, "first transfer must be write.\n");
+                        return -EINVAL;
                 }
-                dev_dbg(&adap->dev, "transfer complete\n");
-                pmsg++;         /* next message */
+                if (msg->len > 3) {
+                        dev_err(dev->dev, "first message size must be <= 3.\n");
+                        return -EINVAL;
+                }
+
+                /* 1st msg is put into the internal address, start with 2nd */
+                m_start = &msg[1];
+                for (i = 0; i < msg->len; ++i) {
+                        const unsigned addr = msg->buf[msg->len - 1 - i];
+
+                        internal_address |= addr << (8 * i);
+                        int_addr_flag += AT91_TWI_IADRSZ_1;
+                }
+                at91_twi_write(dev, AT91_TWI_IADR, internal_address);
         }
-        return i;
+
+        at91_twi_write(dev, AT91_TWI_MMR, (m_start->addr << 16) | int_addr_flag
+                       | ((m_start->flags & I2C_M_RD) ? AT91_TWI_MREAD : 0));
+
+        dev->buf_len = m_start->len;
+        dev->buf = m_start->buf;
+        dev->msg = m_start;
+
+        ret = at91_do_twi_transfer(dev);
+
+        return (ret < 0) ? ret : num;
 }
 
-/*
- * Return list of supported functionality.
- */
-static u32 at91_func(struct i2c_adapter *adapter)
+static u32 at91_twi_func(struct i2c_adapter *adapter)
 {
-        return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+        return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL
+                | I2C_FUNC_SMBUS_READ_BLOCK_DATA;
 }
 
-static struct i2c_algorithm at91_algorithm = {
-        .master_xfer    = at91_xfer,
-        .functionality  = at91_func,
+static struct i2c_algorithm at91_twi_algorithm = {
+        .master_xfer    = at91_twi_xfer,
+        .functionality  = at91_twi_func,
 };
 
-/*
- * Main initialization routine.
- */
-static int __devinit at91_i2c_probe(struct platform_device *pdev)
-{
-        struct i2c_adapter *adapter;
-        struct resource *res;
-        int rc;
+static struct at91_twi_pdata at91rm9200_config = {
+        .clk_max_div = 5,
+        .clk_offset = 3,
+        .has_unre_flag = true,
+};
 
-        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!res)
-                return -ENXIO;
+static struct at91_twi_pdata at91sam9261_config = {
+        .clk_max_div = 5,
+        .clk_offset = 4,
+        .has_unre_flag = false,
+};
 
-        if (!request_mem_region(res->start, resource_size(res), "at91_i2c"))
-                return -EBUSY;
+static struct at91_twi_pdata at91sam9260_config = {
+        .clk_max_div = 7,
+        .clk_offset = 4,
+        .has_unre_flag = false,
+};
+
+static struct at91_twi_pdata at91sam9g20_config = {
+        .clk_max_div = 7,
+        .clk_offset = 4,
+        .has_unre_flag = false,
+};
+
+static struct at91_twi_pdata at91sam9g10_config = {
+        .clk_max_div = 7,
+        .clk_offset = 4,
+        .has_unre_flag = false,
+};
 
-        twi_base = ioremap(res->start, resource_size(res));
-        if (!twi_base) {
-                rc = -ENOMEM;
-                goto fail0;
+static struct at91_twi_pdata at91sam9x5_config = {
+        .clk_max_div = 7,
+        .clk_offset = 4,
+        .has_unre_flag = false,
+};
+
+static const struct platform_device_id at91_twi_devtypes[] = {
+        {
+                .name = "i2c-at91rm9200",
+                .driver_data = (unsigned long) &at91rm9200_config,
+        }, {
+                .name = "i2c-at91sam9261",
+                .driver_data = (unsigned long) &at91sam9261_config,
+        }, {
+                .name = "i2c-at91sam9260",
+                .driver_data = (unsigned long) &at91sam9260_config,
+        }, {
+                .name = "i2c-at91sam9g20",
+                .driver_data = (unsigned long) &at91sam9g20_config,
+        }, {
+                .name = "i2c-at91sam9g10",
+                .driver_data = (unsigned long) &at91sam9g10_config,
+        }, {
+                /* sentinel */
         }
+};
 
-        twi_clk = clk_get(NULL, "twi_clk");
-        if (IS_ERR(twi_clk)) {
-                dev_err(&pdev->dev, "no clock defined\n");
-                rc = -ENODEV;
-                goto fail1;
+#if defined(CONFIG_OF)
+static const struct of_device_id atmel_twi_dt_ids[] = {
+        {
+                .compatible = "atmel,at91sam9260-i2c",
+                .data = &at91sam9260_config,
+        } , {
+                .compatible = "atmel,at91sam9g20-i2c",
+                .data = &at91sam9g20_config,
+        } , {
+                .compatible = "atmel,at91sam9g10-i2c",
+                .data = &at91sam9g10_config,
+        }, {
+                .compatible = "atmel,at91sam9x5-i2c",
+                .data = &at91sam9x5_config,
+        }, {
+                /* sentinel */
         }
+};
+MODULE_DEVICE_TABLE(of, atmel_twi_dt_ids);
+#else
+#define atmel_twi_dt_ids NULL
+#endif
 
-        adapter = kzalloc(sizeof(struct i2c_adapter), GFP_KERNEL);
-        if (adapter == NULL) {
-                dev_err(&pdev->dev, "can't allocate inteface!\n");
-                rc = -ENOMEM;
-                goto fail2;
+static struct at91_twi_pdata * __devinit at91_twi_get_driver_data(
+                                        struct platform_device *pdev)
+{
+        if (pdev->dev.of_node) {
+                const struct of_device_id *match;
+                match = of_match_node(atmel_twi_dt_ids, pdev->dev.of_node);
+                if (!match)
+                        return NULL;
+                return match->data;
         }
-        snprintf(adapter->name, sizeof(adapter->name), "AT91");
-        adapter->algo = &at91_algorithm;
-        adapter->class = I2C_CLASS_HWMON;
-        adapter->dev.parent = &pdev->dev;
-        /* adapter->id == 0 ... only one TWI controller for now */
+        return (struct at91_twi_pdata *) platform_get_device_id(pdev)->driver_data;
+}
+
+static int __devinit at91_twi_probe(struct platform_device *pdev)
+{
+        struct at91_twi_dev *dev;
+        struct resource *mem;
+        int rc;
+
+        dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+        if (!dev)
+                return -ENOMEM;
+        init_completion(&dev->cmd_complete);
+        dev->dev = &pdev->dev;
+
+        mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!mem)
+                return -ENODEV;
+
+        dev->pdata = at91_twi_get_driver_data(pdev);
+        if (!dev->pdata)
+                return -ENODEV;
 
-        platform_set_drvdata(pdev, adapter);
+        dev->base = devm_request_and_ioremap(&pdev->dev, mem);
+        if (!dev->base)
+                return -EBUSY;
 
-        clk_enable(twi_clk);            /* enable peripheral clock */
-        at91_twi_hwinit();              /* initialize TWI controller */
+        dev->irq = platform_get_irq(pdev, 0);
+        if (dev->irq < 0)
+                return dev->irq;
 
-        rc = i2c_add_numbered_adapter(adapter);
+        rc = devm_request_irq(&pdev->dev, dev->irq, atmel_twi_interrupt, 0,
+                         dev_name(dev->dev), dev);
         if (rc) {
-                dev_err(&pdev->dev, "Adapter %s registration failed\n",
-                                adapter->name);
-                goto fail3;
+                dev_err(dev->dev, "Cannot get irq %d: %d\n", dev->irq, rc);
+                return rc;
         }
 
-        dev_info(&pdev->dev, "AT91 i2c bus driver.\n");
-        return 0;
+        platform_set_drvdata(pdev, dev);
 
-fail3:
-        platform_set_drvdata(pdev, NULL);
-        kfree(adapter);
-        clk_disable(twi_clk);
-fail2:
-        clk_put(twi_clk);
-fail1:
-        iounmap(twi_base);
-fail0:
-        release_mem_region(res->start, resource_size(res));
+        dev->clk = devm_clk_get(dev->dev, NULL);
+        if (IS_ERR(dev->clk)) {
+                dev_err(dev->dev, "no clock defined\n");
+                return -ENODEV;
+        }
+        clk_prepare_enable(dev->clk);
+
+        at91_calc_twi_clock(dev, TWI_CLK_HZ);
+        at91_init_twi_bus(dev);
+
+        snprintf(dev->adapter.name, sizeof(dev->adapter.name), "AT91");
+        i2c_set_adapdata(&dev->adapter, dev);
+        dev->adapter.owner = THIS_MODULE;
+        dev->adapter.class = I2C_CLASS_HWMON;
+        dev->adapter.algo = &at91_twi_algorithm;
+        dev->adapter.dev.parent = dev->dev;
+        dev->adapter.nr = pdev->id;
+        dev->adapter.timeout = AT91_I2C_TIMEOUT;
+        dev->adapter.dev.of_node = pdev->dev.of_node;
+
+        rc = i2c_add_numbered_adapter(&dev->adapter);
+        if (rc) {
+                dev_err(dev->dev, "Adapter %s registration failed\n",
+                        dev->adapter.name);
+                clk_disable_unprepare(dev->clk);
+                return rc;
+        }
 
-        return rc;
+        of_i2c_register_devices(&dev->adapter);
+
+        dev_info(dev->dev, "AT91 i2c bus driver.\n");
+        return 0;
 }
 
-static int __devexit at91_i2c_remove(struct platform_device *pdev)
+static int __devexit at91_twi_remove(struct platform_device *pdev)
 {
-        struct i2c_adapter *adapter = platform_get_drvdata(pdev);
-        struct resource *res;
+        struct at91_twi_dev *dev = platform_get_drvdata(pdev);
         int rc;
 
-        rc = i2c_del_adapter(adapter);
-        platform_set_drvdata(pdev, NULL);
-
-        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        iounmap(twi_base);
-        release_mem_region(res->start, resource_size(res));
-
-        clk_disable(twi_clk);           /* disable peripheral clock */
-        clk_put(twi_clk);
+        rc = i2c_del_adapter(&dev->adapter);
+        clk_disable_unprepare(dev->clk);
 
         return rc;
 }
 
 #ifdef CONFIG_PM
 
-/* NOTE: could save a few mA by keeping clock off outside of at91_xfer... */
-
-static int at91_i2c_suspend(struct device *dev)
+static int at91_twi_runtime_suspend(struct device *dev)
 {
-        clk_disable(twi_clk);
+        struct at91_twi_dev *twi_dev = dev_get_drvdata(dev);
+
+        clk_disable(twi_dev->clk);
+
         return 0;
 }
 
-static int at91_i2c_resume(struct device *dev)
+static int at91_twi_runtime_resume(struct device *dev)
 {
-        return clk_enable(twi_clk);
+        struct at91_twi_dev *twi_dev = dev_get_drvdata(dev);
+
+        return clk_enable(twi_dev->clk);
 }
 
-static SIMPLE_DEV_PM_OPS(at91_i2c_pm, at91_i2c_suspend, at91_i2c_resume);
-#define AT91_I2C_PM     (&at91_i2c_pm)
+static const struct dev_pm_ops at91_twi_pm = {
+        .runtime_suspend        = at91_twi_runtime_suspend,
+        .runtime_resume         = at91_twi_runtime_resume,
+};
 
+#define at91_twi_pm_ops (&at91_twi_pm)
 #else
-#define AT91_I2C_PM     NULL
+#define at91_twi_pm_ops NULL
 #endif
 
-static struct platform_driver at91_i2c_driver = {
-        .probe          = at91_i2c_probe,
-        .remove         = __devexit_p(at91_i2c_remove),
+static struct platform_driver at91_twi_driver = {
+        .probe          = at91_twi_probe,
+        .remove         = __devexit_p(at91_twi_remove),
+        .id_table       = at91_twi_devtypes,
         .driver         = {
                 .name   = "at91_i2c",
                 .owner  = THIS_MODULE,
-                .pm     = AT91_I2C_PM,
+                .of_match_table = atmel_twi_dt_ids,
+                .pm     = at91_twi_pm_ops,
         },
 };
 
-module_platform_driver(at91_i2c_driver);
+static int __init at91_twi_init(void)
+{
+        return platform_driver_register(&at91_twi_driver);
+}
+
+static void __exit at91_twi_exit(void)
+{
+        platform_driver_unregister(&at91_twi_driver);
+}
+
+subsys_initcall(at91_twi_init);
+module_exit(at91_twi_exit);
 
-MODULE_AUTHOR("Rick Bronson");
+MODULE_AUTHOR("Nikolaus Voss <n.voss@weinmann.de>");
 MODULE_DESCRIPTION("I2C (TWI) driver for Atmel AT91");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:at91_i2c");
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index 79a2542d8c41..6a0a55319449 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -38,6 +38,8 @@
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
 #include <linux/gpio.h>
+#include <linux/of_i2c.h>
+#include <linux/of_device.h>
 
 #include <mach/hardware.h>
 #include <linux/platform_data/i2c-davinci.h>
@@ -114,6 +116,7 @@ struct davinci_i2c_dev {
         struct completion       xfr_complete;
         struct notifier_block   freq_transition;
 #endif
+        struct davinci_i2c_platform_data *pdata;
 };
 
 /* default platform data to use if not supplied in the platform_device */
@@ -155,7 +158,7 @@ static void generic_i2c_clock_pulse(unsigned int scl_pin)
 static void i2c_recover_bus(struct davinci_i2c_dev *dev)
 {
         u32 flag = 0;
-        struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
+        struct davinci_i2c_platform_data *pdata = dev->pdata;
 
         dev_err(dev->dev, "initiating i2c bus recovery\n");
         /* Send NACK to the slave */
@@ -163,8 +166,7 @@ static void i2c_recover_bus(struct davinci_i2c_dev *dev)
         flag |=  DAVINCI_I2C_MDR_NACK;
         /* write the data into mode register */
         davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, flag);
-        if (pdata)
-                generic_i2c_clock_pulse(pdata->scl_pin);
+        generic_i2c_clock_pulse(pdata->scl_pin);
         /* Send STOP */
         flag = davinci_i2c_read_reg(dev, DAVINCI_I2C_MDR_REG);
         flag |= DAVINCI_I2C_MDR_STP;
@@ -187,7 +189,7 @@ static inline void davinci_i2c_reset_ctrl(struct davinci_i2c_dev *i2c_dev,
 
 static void i2c_davinci_calc_clk_dividers(struct davinci_i2c_dev *dev)
 {
-        struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
+        struct davinci_i2c_platform_data *pdata = dev->pdata;
         u16 psc;
         u32 clk;
         u32 d;
@@ -235,10 +237,7 @@ static void i2c_davinci_calc_clk_dividers(struct davinci_i2c_dev *dev)
  */
 static int i2c_davinci_init(struct davinci_i2c_dev *dev)
 {
-        struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
-
-        if (!pdata)
-                pdata = &davinci_i2c_platform_data_default;
+        struct davinci_i2c_platform_data *pdata = dev->pdata;
 
         /* put I2C into reset */
         davinci_i2c_reset_ctrl(dev, 0);
@@ -260,6 +259,7 @@ static int i2c_davinci_init(struct davinci_i2c_dev *dev)
         dev_dbg(dev->dev, "bus_freq = %dkHz, bus_delay = %d\n",
                 pdata->bus_freq, pdata->bus_delay);
 
+
         /* Take the I2C module out of reset: */
         davinci_i2c_reset_ctrl(dev, 1);
 
@@ -308,13 +308,11 @@ static int
 i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 {
         struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
-        struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
+        struct davinci_i2c_platform_data *pdata = dev->pdata;
         u32 flag;
         u16 w;
         int r;
 
-        if (!pdata)
-                pdata = &davinci_i2c_platform_data_default;
         /* Introduce a delay, required for some boards (e.g Davinci EVM) */
         if (pdata->bus_delay)
                 udelay(pdata->bus_delay);
@@ -635,6 +633,12 @@ static struct i2c_algorithm i2c_davinci_algo = {
         .functionality  = i2c_davinci_func,
 };
 
+static const struct of_device_id davinci_i2c_of_match[] = {
+        {.compatible = "ti,davinci-i2c", },
+        {},
+};
+MODULE_DEVICE_TABLE(of, davinci_i2c_of_match);
+
 static int davinci_i2c_probe(struct platform_device *pdev)
 {
         struct davinci_i2c_dev *dev;
@@ -674,14 +678,33 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 #endif
         dev->dev = get_device(&pdev->dev);
         dev->irq = irq->start;
+        dev->pdata = dev->dev->platform_data;
         platform_set_drvdata(pdev, dev);
 
+        if (!dev->pdata && pdev->dev.of_node) {
+                u32 prop;
+
+                dev->pdata = devm_kzalloc(&pdev->dev,
+                        sizeof(struct davinci_i2c_platform_data), GFP_KERNEL);
+                if (!dev->pdata) {
+                        r = -ENOMEM;
+                        goto err_free_mem;
+                }
+                memcpy(dev->pdata, &davinci_i2c_platform_data_default,
+                        sizeof(struct davinci_i2c_platform_data));
+                if (!of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+                        &prop))
+                        dev->pdata->bus_freq = prop / 1000;
+        } else if (!dev->pdata) {
+                dev->pdata = &davinci_i2c_platform_data_default;
+        }
+
         dev->clk = clk_get(&pdev->dev, NULL);
         if (IS_ERR(dev->clk)) {
                 r = -ENODEV;
                 goto err_free_mem;
         }
-        clk_enable(dev->clk);
+        clk_prepare_enable(dev->clk);
 
         dev->base = ioremap(mem->start, resource_size(mem));
         if (!dev->base) {
@@ -711,6 +734,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
         adap->algo = &i2c_davinci_algo;
         adap->dev.parent = &pdev->dev;
         adap->timeout = DAVINCI_I2C_TIMEOUT;
+        adap->dev.of_node = pdev->dev.of_node;
 
         adap->nr = pdev->id;
         r = i2c_add_numbered_adapter(adap);
@@ -718,6 +742,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "failure adding adapter\n");
                 goto err_free_irq;
         }
+        of_i2c_register_devices(adap);
 
         return 0;
 
@@ -726,7 +751,7 @@ err_free_irq:
 err_unuse_clocks:
         iounmap(dev->base);
 err_mem_ioremap:
-        clk_disable(dev->clk);
+        clk_disable_unprepare(dev->clk);
         clk_put(dev->clk);
         dev->clk = NULL;
 err_free_mem:
@@ -750,7 +775,7 @@ static int davinci_i2c_remove(struct platform_device *pdev)
         i2c_del_adapter(&dev->adapter);
         put_device(&pdev->dev);
 
-        clk_disable(dev->clk);
+        clk_disable_unprepare(dev->clk);
         clk_put(dev->clk);
         dev->clk = NULL;
 
@@ -772,7 +797,7 @@ static int davinci_i2c_suspend(struct device *dev)
 
         /* put I2C into reset */
         davinci_i2c_reset_ctrl(i2c_dev, 0);
-        clk_disable(i2c_dev->clk);
+        clk_disable_unprepare(i2c_dev->clk);
 
         return 0;
 }
@@ -782,7 +807,7 @@ static int davinci_i2c_resume(struct device *dev)
         struct platform_device *pdev = to_platform_device(dev);
         struct davinci_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 
-        clk_enable(i2c_dev->clk);
+        clk_prepare_enable(i2c_dev->clk);
         /* take I2C out of reset */
         davinci_i2c_reset_ctrl(i2c_dev, 1);
 
@@ -809,6 +834,7 @@ static struct platform_driver davinci_i2c_driver = {
                 .name   = "i2c_davinci",
                 .owner  = THIS_MODULE,
                 .pm     = davinci_i2c_pm_ops,
+                .of_match_table = of_match_ptr(davinci_i2c_of_match),
         },
 };
 
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 37793156bd93..6abc00d59881 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -82,7 +82,8 @@
 #include <linux/wait.h>
 #include <linux/err.h>
 
-#if defined CONFIG_I2C_MUX || defined CONFIG_I2C_MUX_MODULE
+#if (defined CONFIG_I2C_MUX_GPIO || defined CONFIG_I2C_MUX_GPIO_MODULE) && \
+                defined CONFIG_DMI
 #include <linux/gpio.h>
 #include <linux/i2c-mux-gpio.h>
 #include <linux/platform_device.h>
@@ -192,7 +193,8 @@ struct i801_priv {
         int len;
         u8 *data;
 
-#if defined CONFIG_I2C_MUX || defined CONFIG_I2C_MUX_MODULE
+#if (defined CONFIG_I2C_MUX_GPIO || defined CONFIG_I2C_MUX_GPIO_MODULE) && \
+                defined CONFIG_DMI
         const struct i801_mux_config *mux_drvdata;
         struct platform_device *mux_pdev;
 #endif
@@ -921,7 +923,8 @@ static void __init input_apanel_init(void) {}
 static void __devinit i801_probe_optional_slaves(struct i801_priv *priv) {}
 #endif  /* CONFIG_X86 && CONFIG_DMI */
 
-#if defined CONFIG_I2C_MUX || defined CONFIG_I2C_MUX_MODULE
+#if (defined CONFIG_I2C_MUX_GPIO || defined CONFIG_I2C_MUX_GPIO_MODULE) && \
+                defined CONFIG_DMI
 static struct i801_mux_config i801_mux_config_asus_z8_d12 = {
         .gpio_chip = "gpio_ich",
         .values = { 0x02, 0x03 },
@@ -1059,7 +1062,7 @@ static unsigned int __devinit i801_get_adapter_class(struct i801_priv *priv)
 
         id = dmi_first_match(mux_dmi_table);
         if (id) {
-                /* Remove from branch classes from trunk */
+                /* Remove branch classes from trunk */
                 mux_config = id->driver_data;
                 for (i = 0; i < mux_config->n_values; i++)
                         class &= ~mux_config->classes[i];
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index b7907ba7448a..2ef162d148cb 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -272,9 +272,9 @@ static void __init i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx,
 
         /* dev_dbg() can't be used, because adapter is not yet registered */
 #ifdef CONFIG_I2C_DEBUG_BUS
-        printk(KERN_DEBUG "I2C: <%s> I2C_CLK=%d, REQ DIV=%d\n",
+        dev_dbg(&i2c_imx->adapter.dev, "<%s> I2C_CLK=%d, REQ DIV=%d\n",
                 __func__, i2c_clk_rate, div);
-        printk(KERN_DEBUG "I2C: <%s> IFDR[IC]=0x%x, REAL DIV=%d\n",
+        dev_dbg(&i2c_imx->adapter.dev, "<%s> IFDR[IC]=0x%x, REAL DIV=%d\n",
                 __func__, i2c_clk_div[i][1], i2c_clk_div[i][0]);
 #endif
 }
@@ -564,7 +564,7 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
                 resource_size(res), res->start);
         dev_dbg(&i2c_imx->adapter.dev, "adapter name: \"%s\"\n",
                 i2c_imx->adapter.name);
-        dev_dbg(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
+        dev_info(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
 
         return 0;   /* Return OK */
 }
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index 57f7703ce2e8..ca86430cb4a2 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -576,7 +576,23 @@ static int mpc_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
                             mpc_write(i2c, pmsg->addr, pmsg->buf, pmsg->len, i);
                 }
         }
-        mpc_i2c_stop(i2c);
+        mpc_i2c_stop(i2c); /* Initiate STOP */
+        orig_jiffies = jiffies;
+        /* Wait until STOP is seen, allow up to 1 s */
+        while (readb(i2c->base + MPC_I2C_SR) & CSR_MBB) {
+                if (time_after(jiffies, orig_jiffies + HZ)) {
+                        u8 status = readb(i2c->base + MPC_I2C_SR);
+
+                        dev_dbg(i2c->dev, "timeout\n");
+                        if ((status & (CSR_MCF | CSR_MBB | CSR_RXAK)) != 0) {
+                                writeb(status & ~CSR_MAL,
+                                       i2c->base + MPC_I2C_SR);
+                                mpc_i2c_fixup(i2c);
+                        }
+                        return -EIO;
+                }
+                cond_resched();
+        }
         return (ret < 0) ? ret : num;
 }
 
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 51f05b8520ed..286ca1917820 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -1,14 +1,12 @@
 /*
  * Freescale MXS I2C bus driver
  *
- * Copyright (C) 2011 Wolfram Sang, Pengutronix e.K.
+ * Copyright (C) 2011-2012 Wolfram Sang, Pengutronix e.K.
  *
  * based on a (non-working) driver which was:
  *
  * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  *
- * TODO: add dma-support if platform-support for it is available
- *
  * 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
@@ -31,6 +29,9 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_i2c.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/fsl/mxs-dma.h>
 
 #define DRIVER_NAME "mxs-i2c"
 
@@ -70,23 +71,6 @@
                                  MXS_I2C_CTRL1_SLAVE_STOP_IRQ | \
                                  MXS_I2C_CTRL1_SLAVE_IRQ)
 
-#define MXS_I2C_QUEUECTRL       (0x60)
-#define MXS_I2C_QUEUECTRL_SET   (0x64)
-#define MXS_I2C_QUEUECTRL_CLR   (0x68)
-
-#define MXS_I2C_QUEUECTRL_QUEUE_RUN             0x20
-#define MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE        0x04
-
-#define MXS_I2C_QUEUESTAT       (0x70)
-#define MXS_I2C_QUEUESTAT_RD_QUEUE_EMPTY        0x00002000
-#define MXS_I2C_QUEUESTAT_WRITE_QUEUE_CNT_MASK  0x0000001F
-
-#define MXS_I2C_QUEUECMD        (0x80)
-
-#define MXS_I2C_QUEUEDATA       (0x90)
-
-#define MXS_I2C_DATA            (0xa0)
-
 
 #define MXS_CMD_I2C_SELECT      (MXS_I2C_CTRL0_RETAIN_CLOCK |   \
                                  MXS_I2C_CTRL0_PRE_SEND_START | \
@@ -146,6 +130,15 @@ struct mxs_i2c_dev {
         u32 cmd_err;
         struct i2c_adapter adapter;
         const struct mxs_i2c_speed_config *speed;
+
+        /* DMA support components */
+        int                             dma_channel;
+        struct dma_chan                 *dmach;
+        struct mxs_dma_data             dma_data;
+        uint32_t                        pio_data[2];
+        uint32_t                        addr_data;
+        struct scatterlist              sg_io[2];
+        bool                            dma_read;
 };
 
 static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
@@ -157,95 +150,150 @@ static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
         writel(i2c->speed->timing2, i2c->regs + MXS_I2C_TIMING2);
 
         writel(MXS_I2C_IRQ_MASK << 8, i2c->regs + MXS_I2C_CTRL1_SET);
-        writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
-                        i2c->regs + MXS_I2C_QUEUECTRL_SET);
 }
 
-static void mxs_i2c_pioq_setup_read(struct mxs_i2c_dev *i2c, u8 addr, int len,
-                                        int flags)
+static void mxs_i2c_dma_finish(struct mxs_i2c_dev *i2c)
 {
-        u32 data;
-
-        writel(MXS_CMD_I2C_SELECT, i2c->regs + MXS_I2C_QUEUECMD);
-
-        data = (addr << 1) | I2C_SMBUS_READ;
-        writel(data, i2c->regs + MXS_I2C_DATA);
-
-        data = MXS_CMD_I2C_READ | MXS_I2C_CTRL0_XFER_COUNT(len) | flags;
-        writel(data, i2c->regs + MXS_I2C_QUEUECMD);
+        if (i2c->dma_read) {
+                dma_unmap_sg(i2c->dev, &i2c->sg_io[0], 1, DMA_TO_DEVICE);
+                dma_unmap_sg(i2c->dev, &i2c->sg_io[1], 1, DMA_FROM_DEVICE);
+        } else {
+                dma_unmap_sg(i2c->dev, i2c->sg_io, 2, DMA_TO_DEVICE);
+        }
 }
 
-static void mxs_i2c_pioq_setup_write(struct mxs_i2c_dev *i2c,
-                                    u8 addr, u8 *buf, int len, int flags)
+static void mxs_i2c_dma_irq_callback(void *param)
 {
-        u32 data;
-        int i, shifts_left;
-
-        data = MXS_CMD_I2C_WRITE | MXS_I2C_CTRL0_XFER_COUNT(len + 1) | flags;
-        writel(data, i2c->regs + MXS_I2C_QUEUECMD);
-
-        /*
-         * We have to copy the slave address (u8) and buffer (arbitrary number
-         * of u8) into the data register (u32). To achieve that, the u8 are put
-         * into the MSBs of 'data' which is then shifted for the next u8. When
-         * appropriate, 'data' is written to MXS_I2C_DATA. So, the first u32
-         * looks like this:
-         *
-         *  3          2          1          0
-         * 10987654|32109876|54321098|76543210
-         * --------+--------+--------+--------
-         * buffer+2|buffer+1|buffer+0|slave_addr
-         */
-
-        data = ((addr << 1) | I2C_SMBUS_WRITE) << 24;
+        struct mxs_i2c_dev *i2c = param;
 
-        for (i = 0; i < len; i++) {
-                data >>= 8;
-                data |= buf[i] << 24;
-                if ((i & 3) == 2)
-                        writel(data, i2c->regs + MXS_I2C_DATA);
-        }
-
-        /* Write out the remaining bytes if any */
-        shifts_left = 24 - (i & 3) * 8;
-        if (shifts_left)
-                writel(data >> shifts_left, i2c->regs + MXS_I2C_DATA);
+        complete(&i2c->cmd_complete);
+        mxs_i2c_dma_finish(i2c);
 }
 
-/*
- * TODO: should be replaceable with a waitqueue and RD_QUEUE_IRQ (setting the
- * rd_threshold to 1). Couldn't get this to work, though.
- */
-static int mxs_i2c_wait_for_data(struct mxs_i2c_dev *i2c)
+static int mxs_i2c_dma_setup_xfer(struct i2c_adapter *adap,
+                        struct i2c_msg *msg, uint32_t flags)
 {
-        unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+        struct dma_async_tx_descriptor *desc;
+        struct mxs_i2c_dev *i2c = i2c_get_adapdata(adap);
 
-        while (readl(i2c->regs + MXS_I2C_QUEUESTAT)
-                        & MXS_I2C_QUEUESTAT_RD_QUEUE_EMPTY) {
-                        if (time_after(jiffies, timeout))
-                                return -ETIMEDOUT;
-                        cond_resched();
-        }
+        if (msg->flags & I2C_M_RD) {
+                i2c->dma_read = 1;
+                i2c->addr_data = (msg->addr << 1) | I2C_SMBUS_READ;
+
+                /*
+                 * SELECT command.
+                 */
+
+                /* Queue the PIO register write transfer. */
+                i2c->pio_data[0] = MXS_CMD_I2C_SELECT;
+                desc = dmaengine_prep_slave_sg(i2c->dmach,
+                                        (struct scatterlist *)&i2c->pio_data[0],
+                                        1, DMA_TRANS_NONE, 0);
+                if (!desc) {
+                        dev_err(i2c->dev,
+                                "Failed to get PIO reg. write descriptor.\n");
+                        goto select_init_pio_fail;
+                }
 
-        return 0;
-}
+                /* Queue the DMA data transfer. */
+                sg_init_one(&i2c->sg_io[0], &i2c->addr_data, 1);
+                dma_map_sg(i2c->dev, &i2c->sg_io[0], 1, DMA_TO_DEVICE);
+                desc = dmaengine_prep_slave_sg(i2c->dmach, &i2c->sg_io[0], 1,
+                                        DMA_MEM_TO_DEV,
+                                        DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!desc) {
+                        dev_err(i2c->dev,
+                                "Failed to get DMA data write descriptor.\n");
+                        goto select_init_dma_fail;
+                }
 
-static int mxs_i2c_finish_read(struct mxs_i2c_dev *i2c, u8 *buf, int len)
-{
-        u32 uninitialized_var(data);
-        int i;
+                /*
+                 * READ command.
+                 */
+
+                /* Queue the PIO register write transfer. */
+                i2c->pio_data[1] = flags | MXS_CMD_I2C_READ |
+                                MXS_I2C_CTRL0_XFER_COUNT(msg->len);
+                desc = dmaengine_prep_slave_sg(i2c->dmach,
+                                        (struct scatterlist *)&i2c->pio_data[1],
+                                        1, DMA_TRANS_NONE, DMA_PREP_INTERRUPT);
+                if (!desc) {
+                        dev_err(i2c->dev,
+                                "Failed to get PIO reg. write descriptor.\n");
+                        goto select_init_dma_fail;
+                }
+
+                /* Queue the DMA data transfer. */
+                sg_init_one(&i2c->sg_io[1], msg->buf, msg->len);
+                dma_map_sg(i2c->dev, &i2c->sg_io[1], 1, DMA_FROM_DEVICE);
+                desc = dmaengine_prep_slave_sg(i2c->dmach, &i2c->sg_io[1], 1,
+                                        DMA_DEV_TO_MEM,
+                                        DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!desc) {
+                        dev_err(i2c->dev,
+                                "Failed to get DMA data write descriptor.\n");
+                        goto read_init_dma_fail;
+                }
+        } else {
+                i2c->dma_read = 0;
+                i2c->addr_data = (msg->addr << 1) | I2C_SMBUS_WRITE;
+
+                /*
+                 * WRITE command.
+                 */
+
+                /* Queue the PIO register write transfer. */
+                i2c->pio_data[0] = flags | MXS_CMD_I2C_WRITE |
+                                MXS_I2C_CTRL0_XFER_COUNT(msg->len + 1);
+                desc = dmaengine_prep_slave_sg(i2c->dmach,
+                                        (struct scatterlist *)&i2c->pio_data[0],
+                                        1, DMA_TRANS_NONE, 0);
+                if (!desc) {
+                        dev_err(i2c->dev,
+                                "Failed to get PIO reg. write descriptor.\n");
+                        goto write_init_pio_fail;
+                }
 
-        for (i = 0; i < len; i++) {
-                if ((i & 3) == 0) {
-                        if (mxs_i2c_wait_for_data(i2c))
-                                return -ETIMEDOUT;
-                        data = readl(i2c->regs + MXS_I2C_QUEUEDATA);
+                /* Queue the DMA data transfer. */
+                sg_init_table(i2c->sg_io, 2);
+                sg_set_buf(&i2c->sg_io[0], &i2c->addr_data, 1);
+                sg_set_buf(&i2c->sg_io[1], msg->buf, msg->len);
+                dma_map_sg(i2c->dev, i2c->sg_io, 2, DMA_TO_DEVICE);
+                desc = dmaengine_prep_slave_sg(i2c->dmach, i2c->sg_io, 2,
+                                        DMA_MEM_TO_DEV,
+                                        DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!desc) {
+                        dev_err(i2c->dev,
+                                "Failed to get DMA data write descriptor.\n");
+                        goto write_init_dma_fail;
                 }
-                buf[i] = data & 0xff;
-                data >>= 8;
         }
 
+        /*
+         * The last descriptor must have this callback,
+         * to finish the DMA transaction.
+         */
+        desc->callback = mxs_i2c_dma_irq_callback;
+        desc->callback_param = i2c;
+
+        /* Start the transfer. */
+        dmaengine_submit(desc);
+        dma_async_issue_pending(i2c->dmach);
         return 0;
+
+/* Read failpath. */
+read_init_dma_fail:
+        dma_unmap_sg(i2c->dev, &i2c->sg_io[1], 1, DMA_FROM_DEVICE);
+select_init_dma_fail:
+        dma_unmap_sg(i2c->dev, &i2c->sg_io[0], 1, DMA_TO_DEVICE);
+select_init_pio_fail:
+        return -EINVAL;
+
+/* Write failpath. */
+write_init_dma_fail:
+        dma_unmap_sg(i2c->dev, i2c->sg_io, 2, DMA_TO_DEVICE);
+write_init_pio_fail:
+        return -EINVAL;
 }
 
 /*
@@ -258,6 +306,8 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
         int ret;
         int flags;
 
+        flags = stop ? MXS_I2C_CTRL0_POST_SEND_STOP : 0;
+
         dev_dbg(i2c->dev, "addr: 0x%04x, len: %d, flags: 0x%x, stop: %d\n",
                 msg->addr, msg->len, msg->flags, stop);
 
@@ -267,33 +317,17 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
         init_completion(&i2c->cmd_complete);
         i2c->cmd_err = 0;
 
-        flags = stop ? MXS_I2C_CTRL0_POST_SEND_STOP : 0;
-
-        if (msg->flags & I2C_M_RD)
-                mxs_i2c_pioq_setup_read(i2c, msg->addr, msg->len, flags);
-        else
-                mxs_i2c_pioq_setup_write(i2c, msg->addr, msg->buf, msg->len,
-                                        flags);
-
-        writel(MXS_I2C_QUEUECTRL_QUEUE_RUN,
-                        i2c->regs + MXS_I2C_QUEUECTRL_SET);
+        ret = mxs_i2c_dma_setup_xfer(adap, msg, flags);
+        if (ret)
+                return ret;
 
         ret = wait_for_completion_timeout(&i2c->cmd_complete,
                                                 msecs_to_jiffies(1000));
         if (ret == 0)
                 goto timeout;
 
-        if ((!i2c->cmd_err) && (msg->flags & I2C_M_RD)) {
-                ret = mxs_i2c_finish_read(i2c, msg->buf, msg->len);
-                if (ret)
-                        goto timeout;
-        }
-
         if (i2c->cmd_err == -ENXIO)
                 mxs_i2c_reset(i2c);
-        else
-                writel(MXS_I2C_QUEUECTRL_QUEUE_RUN,
-                                i2c->regs + MXS_I2C_QUEUECTRL_CLR);
 
         dev_dbg(i2c->dev, "Done with err=%d\n", i2c->cmd_err);
 
@@ -301,6 +335,7 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 
 timeout:
         dev_dbg(i2c->dev, "Timeout!\n");
+        mxs_i2c_dma_finish(i2c);
         mxs_i2c_reset(i2c);
         return -ETIMEDOUT;
 }
@@ -329,7 +364,6 @@ static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
 {
         struct mxs_i2c_dev *i2c = dev_id;
         u32 stat = readl(i2c->regs + MXS_I2C_CTRL1) & MXS_I2C_IRQ_MASK;
-        bool is_last_cmd;
 
         if (!stat)
                 return IRQ_NONE;
@@ -342,12 +376,6 @@ static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
                 /* MXS_I2C_CTRL1_OVERSIZE_XFER_TERM_IRQ is only for slaves */
                 i2c->cmd_err = -EIO;
 
-        is_last_cmd = (readl(i2c->regs + MXS_I2C_QUEUESTAT) &
-                MXS_I2C_QUEUESTAT_WRITE_QUEUE_CNT_MASK) == 0;
-
-        if (is_last_cmd || i2c->cmd_err)
-                complete(&i2c->cmd_complete);
-
         writel(stat, i2c->regs + MXS_I2C_CTRL1_CLR);
 
         return IRQ_HANDLED;
@@ -358,6 +386,21 @@ static const struct i2c_algorithm mxs_i2c_algo = {
         .functionality = mxs_i2c_func,
 };
 
+static bool mxs_i2c_dma_filter(struct dma_chan *chan, void *param)
+{
+        struct mxs_i2c_dev *i2c = param;
+
+        if (!mxs_dma_is_apbx(chan))
+                return false;
+
+        if (chan->chan_id != i2c->dma_channel)
+                return false;
+
+        chan->private = &i2c->dma_data;
+
+        return true;
+}
+
 static int mxs_i2c_get_ofdata(struct mxs_i2c_dev *i2c)
 {
         uint32_t speed;
@@ -365,6 +408,17 @@ static int mxs_i2c_get_ofdata(struct mxs_i2c_dev *i2c)
         struct device_node *node = dev->of_node;
         int ret;
 
+        /*
+         * TODO: This is a temporary solution and should be changed
+         * to use generic DMA binding later when the helpers get in.
+         */
+        ret = of_property_read_u32(node, "fsl,i2c-dma-channel",
+                                   &i2c->dma_channel);
+        if (ret) {
+                dev_err(dev, "Failed to get DMA channel!\n");
+                return -ENODEV;
+        }
+
         ret = of_property_read_u32(node, "clock-frequency", &speed);
         if (ret)
                 dev_warn(dev, "No I2C speed selected, using 100kHz\n");
@@ -384,7 +438,8 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
         struct pinctrl *pinctrl;
         struct resource *res;
         resource_size_t res_size;
-        int err, irq;
+        int err, irq, dmairq;
+        dma_cap_mask_t mask;
 
         pinctrl = devm_pinctrl_get_select_default(dev);
         if (IS_ERR(pinctrl))
@@ -395,7 +450,10 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
                 return -ENOMEM;
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!res)
+        irq = platform_get_irq(pdev, 0);
+        dmairq = platform_get_irq(pdev, 1);
+
+        if (!res || irq < 0 || dmairq < 0)
                 return -ENOENT;
 
         res_size = resource_size(res);
@@ -406,10 +464,6 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
         if (!i2c->regs)
                 return -EBUSY;
 
-        irq = platform_get_irq(pdev, 0);
-        if (irq < 0)
-                return irq;
-
         err = devm_request_irq(dev, irq, mxs_i2c_isr, 0, dev_name(dev), i2c);
         if (err)
                 return err;
@@ -423,6 +477,16 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
                         return err;
         }
 
+        /* Setup the DMA */
+        dma_cap_zero(mask);
+        dma_cap_set(DMA_SLAVE, mask);
+        i2c->dma_data.chan_irq = dmairq;
+        i2c->dmach = dma_request_channel(mask, mxs_i2c_dma_filter, i2c);
+        if (!i2c->dmach) {
+                dev_err(dev, "Failed to request dma\n");
+                return -ENODEV;
+        }
+
         platform_set_drvdata(pdev, i2c);
 
         /* Do reset to enforce correct startup after pinmuxing */
@@ -458,6 +522,9 @@ static int __devexit mxs_i2c_remove(struct platform_device *pdev)
         if (ret)
                 return -EBUSY;
 
+        if (i2c->dmach)
+                dma_release_channel(i2c->dmach);
+
         writel(MXS_I2C_CTRL0_SFTRST, i2c->regs + MXS_I2C_CTRL0_SET);
 
         platform_set_drvdata(pdev, NULL);
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 61b00edacb08..02c3115a2dfa 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -22,9 +22,10 @@
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/io.h>
-#include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_data/i2c-nomadik.h>
+#include <linux/of.h>
+#include <linux/of_i2c.h>
 
 #define DRIVER_NAME "nmk-i2c"
 
@@ -146,7 +147,6 @@ struct i2c_nmk_client {
  * @stop: stop condition.
  * @xfer_complete: acknowledge completion for a I2C message.
  * @result: controller propogated result.
- * @regulator: pointer to i2c regulator.
  * @busy: Busy doing transfer.
  */
 struct nmk_i2c_dev {
@@ -160,7 +160,6 @@ struct nmk_i2c_dev {
         int                             stop;
         struct completion               xfer_complete;
         int                             result;
-        struct regulator                *regulator;
         bool                            busy;
 };
 
@@ -643,11 +642,13 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 
         dev->busy = true;
 
-        if (dev->regulator)
-                regulator_enable(dev->regulator);
         pm_runtime_get_sync(&dev->adev->dev);
 
-        clk_enable(dev->clk);
+        status = clk_prepare_enable(dev->clk);
+        if (status) {
+                dev_err(&dev->adev->dev, "can't prepare_enable clock\n");
+                goto out_clk;
+        }
 
         status = init_hw(dev);
         if (status)
@@ -674,10 +675,9 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
         }
 
 out:
-        clk_disable(dev->clk);
+        clk_disable_unprepare(dev->clk);
+out_clk:
         pm_runtime_put_sync(&dev->adev->dev);
-        if (dev->regulator)
-                regulator_disable(dev->regulator);
 
         dev->busy = false;
 
@@ -920,18 +920,42 @@ static struct nmk_i2c_controller u8500_i2c = {
         .sm             = I2C_FREQ_MODE_FAST,
 };
 
+static void nmk_i2c_of_probe(struct device_node *np,
+                        struct nmk_i2c_controller *pdata)
+{
+        of_property_read_u32(np, "clock-frequency", &pdata->clk_freq);
+
+        /* This driver only supports 'standard' and 'fast' modes of operation. */
+        if (pdata->clk_freq <= 100000)
+                pdata->sm = I2C_FREQ_MODE_STANDARD;
+        else
+                pdata->sm = I2C_FREQ_MODE_FAST;
+}
+
 static atomic_t adapter_id = ATOMIC_INIT(0);
 
 static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 {
         int ret = 0;
         struct nmk_i2c_controller *pdata = adev->dev.platform_data;
+        struct device_node *np = adev->dev.of_node;
         struct nmk_i2c_dev      *dev;
         struct i2c_adapter *adap;
 
-        if (!pdata)
-                /* No i2c configuration found, using the default. */
-                pdata = &u8500_i2c;
+        if (!pdata) {
+                if (np) {
+                        pdata = devm_kzalloc(&adev->dev, sizeof(*pdata), GFP_KERNEL);
+                        if (!pdata) {
+                                ret = -ENOMEM;
+                                goto err_no_mem;
+                        }
+                        /* Provide the default configuration as a base. */
+                        memcpy(pdata, &u8500_i2c, sizeof(struct nmk_i2c_controller));
+                        nmk_i2c_of_probe(np, pdata);
+                } else
+                        /* No i2c configuration found, using the default. */
+                        pdata = &u8500_i2c;
+        }
 
         dev = kzalloc(sizeof(struct nmk_i2c_dev), GFP_KERNEL);
         if (!dev) {
@@ -957,12 +981,6 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
                 goto err_irq;
         }
 
-        dev->regulator = regulator_get(&adev->dev, "v-i2c");
-        if (IS_ERR(dev->regulator)) {
-                dev_warn(&adev->dev, "could not get i2c regulator\n");
-                dev->regulator = NULL;
-        }
-
         pm_suspend_ignore_children(&adev->dev, true);
 
         dev->clk = clk_get(&adev->dev, NULL);
@@ -973,6 +991,7 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
         }
 
         adap = &dev->adap;
+        adap->dev.of_node = np;
         adap->dev.parent = &adev->dev;
         adap->owner     = THIS_MODULE;
         adap->class     = I2C_CLASS_HWMON | I2C_CLASS_SPD;
@@ -1002,6 +1021,8 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
                 goto err_add_adap;
         }
 
+        of_i2c_register_devices(adap);
+
         pm_runtime_put(&adev->dev);
 
         return 0;
@@ -1009,8 +1030,6 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
  err_add_adap:
         clk_put(dev->clk);
  err_no_clk:
-        if (dev->regulator)
-                regulator_put(dev->regulator);
         free_irq(dev->irq, dev);
  err_irq:
         iounmap(dev->virtbase);
@@ -1038,8 +1057,6 @@ static int nmk_i2c_remove(struct amba_device *adev)
         if (res)
                 release_mem_region(res->start, resource_size(res));
         clk_put(dev->clk);
-        if (dev->regulator)
-                regulator_put(dev->regulator);
         pm_runtime_disable(&adev->dev);
         amba_set_drvdata(adev, NULL);
         kfree(dev);
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index a0e49f6aaf96..db31eaed6ea5 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -43,6 +43,7 @@
 #include <linux/slab.h>
 #include <linux/i2c-omap.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_qos.h>
 
 /* I2C controller revisions */
 #define OMAP_I2C_OMAP1_REV_2            0x20
@@ -55,6 +56,9 @@
 /* timeout waiting for the controller to respond */
 #define OMAP_I2C_TIMEOUT (msecs_to_jiffies(1000))
 
+/* timeout for pm runtime autosuspend */
+#define OMAP_I2C_PM_TIMEOUT             1000    /* ms */
+
 /* For OMAP3 I2C_IV has changed to I2C_WE (wakeup enable) */
 enum {
         OMAP_I2C_REV_REG = 0,
@@ -176,15 +180,15 @@ enum {
 #define I2C_OMAP_ERRATA_I462            (1 << 1)
 
 struct omap_i2c_dev {
+        spinlock_t              lock;           /* IRQ synchronization */
         struct device           *dev;
         void __iomem            *base;          /* virtual */
         int                     irq;
         int                     reg_shift;      /* bit shift for I2C register addresses */
         struct completion       cmd_complete;
         struct resource         *ioarea;
-        u32                     latency;        /* maximum mpu wkup latency */
-        void                    (*set_mpu_wkup_lat)(struct device *dev,
-                                                    long latency);
+        u32                     latency;        /* maximum MPU wkup latency */
+        struct pm_qos_request   pm_qos_request;
         u32                     speed;          /* Speed of bus in kHz */
         u32                     dtrev;          /* extra revision from DT */
         u32                     flags;
@@ -193,12 +197,14 @@ struct omap_i2c_dev {
         u8                      *regs;
         size_t                  buf_len;
         struct i2c_adapter      adapter;
+        u8                      threshold;
         u8                      fifo_size;      /* use as flag and value
                                                  * fifo_size==0 implies no fifo
                                                  * if set, should be trsh+1
                                                  */
         u8                      rev;
         unsigned                b_hw:1;         /* bad h/w fixes */
+        unsigned                receiver:1;     /* true when we're in receiver mode */
         u16                     iestate;        /* Saved interrupt register */
         u16                     pscstate;
         u16                     scllstate;
@@ -417,13 +423,6 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
         omap_i2c_write_reg(dev, OMAP_I2C_SCLL_REG, scll);
         omap_i2c_write_reg(dev, OMAP_I2C_SCLH_REG, sclh);
 
-        if (dev->fifo_size) {
-                /* Note: setup required fifo size - 1. RTRSH and XTRSH */
-                buf = (dev->fifo_size - 1) << 8 | OMAP_I2C_BUF_RXFIF_CLR |
-                        (dev->fifo_size - 1) | OMAP_I2C_BUF_TXFIF_CLR;
-                omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, buf);
-        }
-
         /* Take the I2C module out of reset: */
         omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, OMAP_I2C_CON_EN);
 
@@ -461,6 +460,43 @@ static int omap_i2c_wait_for_bb(struct omap_i2c_dev *dev)
         return 0;
 }
 
+static void omap_i2c_resize_fifo(struct omap_i2c_dev *dev, u8 size, bool is_rx)
+{
+        u16             buf;
+
+        if (dev->flags & OMAP_I2C_FLAG_NO_FIFO)
+                return;
+
+        /*
+         * Set up notification threshold based on message size. We're doing
+         * this to try and avoid draining feature as much as possible. Whenever
+         * we have big messages to transfer (bigger than our total fifo size)
+         * then we might use draining feature to transfer the remaining bytes.
+         */
+
+        dev->threshold = clamp(size, (u8) 1, dev->fifo_size);
+
+        buf = omap_i2c_read_reg(dev, OMAP_I2C_BUF_REG);
+
+        if (is_rx) {
+                /* Clear RX Threshold */
+                buf &= ~(0x3f << 8);
+                buf |= ((dev->threshold - 1) << 8) | OMAP_I2C_BUF_RXFIF_CLR;
+        } else {
+                /* Clear TX Threshold */
+                buf &= ~0x3f;
+                buf |= (dev->threshold - 1) | OMAP_I2C_BUF_TXFIF_CLR;
+        }
+
+        omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, buf);
+
+        if (dev->rev < OMAP_I2C_REV_ON_3630_4430)
+                dev->b_hw = 1; /* Enable hardware fixes */
+
+        /* calculate wakeup latency constraint for MPU */
+        dev->latency = (1000000 * dev->threshold) / (1000 * dev->speed / 8);
+}
+
 /*
  * Low level master read/write transaction.
  */
@@ -477,6 +513,9 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
         if (msg->len == 0)
                 return -EINVAL;
 
+        dev->receiver = !!(msg->flags & I2C_M_RD);
+        omap_i2c_resize_fifo(dev, msg->len, dev->receiver);
+
         omap_i2c_write_reg(dev, OMAP_I2C_SA_REG, msg->addr);
 
         /* REVISIT: Could the STB bit of I2C_CON be used with probing? */
@@ -590,8 +629,16 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
         if (r < 0)
                 goto out;
 
-        if (dev->set_mpu_wkup_lat != NULL)
-                dev->set_mpu_wkup_lat(dev->dev, dev->latency);
+        /*
+         * When waiting for completion of a i2c transfer, we need to
+         * set a wake up latency constraint for the MPU. This is to
+         * ensure quick enough wakeup from idle, when transfer
+         * completes.
+         */
+        if (dev->latency)
+                pm_qos_add_request(&dev->pm_qos_request,
+                                   PM_QOS_CPU_DMA_LATENCY,
+                                   dev->latency);
 
         for (i = 0; i < num; i++) {
                 r = omap_i2c_xfer_msg(adap, &msgs[i], (i == (num - 1)));
@@ -599,15 +646,16 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
                         break;
         }
 
-        if (dev->set_mpu_wkup_lat != NULL)
-                dev->set_mpu_wkup_lat(dev->dev, -1);
+        if (dev->latency)
+                pm_qos_remove_request(&dev->pm_qos_request);
 
         if (r == 0)
                 r = num;
 
         omap_i2c_wait_for_bb(dev);
 out:
-        pm_runtime_put(dev->dev);
+        pm_runtime_mark_last_busy(dev->dev);
+        pm_runtime_put_autosuspend(dev->dev);
         return r;
 }
 
@@ -725,186 +773,252 @@ omap_i2c_omap1_isr(int this_irq, void *dev_id)
  * data to DATA_REG. Otherwise some data bytes can be lost while transferring
  * them from the memory to the I2C interface.
  */
-static int errata_omap3_i462(struct omap_i2c_dev *dev, u16 *stat, int *err)
+static int errata_omap3_i462(struct omap_i2c_dev *dev)
 {
         unsigned long timeout = 10000;
+        u16 stat;
 
-        while (--timeout && !(*stat & OMAP_I2C_STAT_XUDF)) {
-                if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
-                        omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
+        do {
+                stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+                if (stat & OMAP_I2C_STAT_XUDF)
+                        break;
+
+                if (stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
+                        omap_i2c_ack_stat(dev, (OMAP_I2C_STAT_XRDY |
                                                         OMAP_I2C_STAT_XDR));
-                        return -ETIMEDOUT;
+                        if (stat & OMAP_I2C_STAT_NACK) {
+                                dev->cmd_err |= OMAP_I2C_STAT_NACK;
+                                omap_i2c_ack_stat(dev, OMAP_I2C_STAT_NACK);
+                        }
+
+                        if (stat & OMAP_I2C_STAT_AL) {
+                                dev_err(dev->dev, "Arbitration lost\n");
+                                dev->cmd_err |= OMAP_I2C_STAT_AL;
+                                omap_i2c_ack_stat(dev, OMAP_I2C_STAT_NACK);
+                        }
+
+                        return -EIO;
                 }
 
                 cpu_relax();
-                *stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
-        }
+        } while (--timeout);
 
         if (!timeout) {
                 dev_err(dev->dev, "timeout waiting on XUDF bit\n");
                 return 0;
         }
 
-        *err |= OMAP_I2C_STAT_XUDF;
         return 0;
 }
 
+static void omap_i2c_receive_data(struct omap_i2c_dev *dev, u8 num_bytes,
+                bool is_rdr)
+{
+        u16             w;
+
+        while (num_bytes--) {
+                w = omap_i2c_read_reg(dev, OMAP_I2C_DATA_REG);
+                *dev->buf++ = w;
+                dev->buf_len--;
+
+                /*
+                 * Data reg in 2430, omap3 and
+                 * omap4 is 8 bit wide
+                 */
+                if (dev->flags & OMAP_I2C_FLAG_16BIT_DATA_REG) {
+                        *dev->buf++ = w >> 8;
+                        dev->buf_len--;
+                }
+        }
+}
+
+static int omap_i2c_transmit_data(struct omap_i2c_dev *dev, u8 num_bytes,
+                bool is_xdr)
+{
+        u16             w;
+
+        while (num_bytes--) {
+                w = *dev->buf++;
+                dev->buf_len--;
+
+                /*
+                 * Data reg in 2430, omap3 and
+                 * omap4 is 8 bit wide
+                 */
+                if (dev->flags & OMAP_I2C_FLAG_16BIT_DATA_REG) {
+                        w |= *dev->buf++ << 8;
+                        dev->buf_len--;
+                }
+
+                if (dev->errata & I2C_OMAP_ERRATA_I462) {
+                        int ret;
+
+                        ret = errata_omap3_i462(dev);
+                        if (ret < 0)
+                                return ret;
+                }
+
+                omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
+        }
+
+        return 0;
+}
+
+static irqreturn_t
+omap_i2c_isr(int irq, void *dev_id)
+{
+        struct omap_i2c_dev *dev = dev_id;
+        irqreturn_t ret = IRQ_HANDLED;
+        u16 mask;
+        u16 stat;
+
+        spin_lock(&dev->lock);
+        mask = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
+        stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+
+        if (stat & mask)
+                ret = IRQ_WAKE_THREAD;
+
+        spin_unlock(&dev->lock);
+
+        return ret;
+}
+
 static irqreturn_t
-omap_i2c_isr(int this_irq, void *dev_id)
+omap_i2c_isr_thread(int this_irq, void *dev_id)
 {
         struct omap_i2c_dev *dev = dev_id;
+        unsigned long flags;
         u16 bits;
-        u16 stat, w;
-        int err, count = 0;
+        u16 stat;
+        int err = 0, count = 0;
+
+        spin_lock_irqsave(&dev->lock, flags);
+        do {
+                bits = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
+                stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+                stat &= bits;
+
+                /* If we're in receiver mode, ignore XDR/XRDY */
+                if (dev->receiver)
+                        stat &= ~(OMAP_I2C_STAT_XDR | OMAP_I2C_STAT_XRDY);
+                else
+                        stat &= ~(OMAP_I2C_STAT_RDR | OMAP_I2C_STAT_RRDY);
 
-        if (pm_runtime_suspended(dev->dev))
-                return IRQ_NONE;
+                if (!stat) {
+                        /* my work here is done */
+                        goto out;
+                }
 
-        bits = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
-        while ((stat = (omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG))) & bits) {
                 dev_dbg(dev->dev, "IRQ (ISR = 0x%04x)\n", stat);
                 if (count++ == 100) {
                         dev_warn(dev->dev, "Too much work in one IRQ\n");
                         break;
                 }
 
-                err = 0;
-complete:
-                /*
-                 * Ack the stat in one go, but [R/X]DR and [R/X]RDY should be
-                 * acked after the data operation is complete.
-                 * Ref: TRM SWPU114Q Figure 18-31
-                 */
-                omap_i2c_write_reg(dev, OMAP_I2C_STAT_REG, stat &
-                                ~(OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
-                                OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
-
-                if (stat & OMAP_I2C_STAT_NACK)
+                if (stat & OMAP_I2C_STAT_NACK) {
                         err |= OMAP_I2C_STAT_NACK;
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_NACK);
+                        break;
+                }
 
                 if (stat & OMAP_I2C_STAT_AL) {
                         dev_err(dev->dev, "Arbitration lost\n");
                         err |= OMAP_I2C_STAT_AL;
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_AL);
+                        break;
                 }
+
                 /*
                  * ProDB0017052: Clear ARDY bit twice
                  */
                 if (stat & (OMAP_I2C_STAT_ARDY | OMAP_I2C_STAT_NACK |
                                         OMAP_I2C_STAT_AL)) {
-                        omap_i2c_ack_stat(dev, stat &
-                                (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
-                                OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR |
-                                OMAP_I2C_STAT_ARDY));
-                        omap_i2c_complete_cmd(dev, err);
-                        return IRQ_HANDLED;
+                        omap_i2c_ack_stat(dev, (OMAP_I2C_STAT_RRDY |
+                                                OMAP_I2C_STAT_RDR |
+                                                OMAP_I2C_STAT_XRDY |
+                                                OMAP_I2C_STAT_XDR |
+                                                OMAP_I2C_STAT_ARDY));
+                        break;
                 }
-                if (stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR)) {
+
+                if (stat & OMAP_I2C_STAT_RDR) {
                         u8 num_bytes = 1;
 
+                        if (dev->fifo_size)
+                                num_bytes = dev->buf_len;
+
+                        omap_i2c_receive_data(dev, num_bytes, true);
+
                         if (dev->errata & I2C_OMAP_ERRATA_I207)
                                 i2c_omap_errata_i207(dev, stat);
 
-                        if (dev->fifo_size) {
-                                if (stat & OMAP_I2C_STAT_RRDY)
-                                        num_bytes = dev->fifo_size;
-                                else    /* read RXSTAT on RDR interrupt */
-                                        num_bytes = (omap_i2c_read_reg(dev,
-                                                        OMAP_I2C_BUFSTAT_REG)
-                                                        >> 8) & 0x3F;
-                        }
-                        while (num_bytes) {
-                                num_bytes--;
-                                w = omap_i2c_read_reg(dev, OMAP_I2C_DATA_REG);
-                                if (dev->buf_len) {
-                                        *dev->buf++ = w;
-                                        dev->buf_len--;
-                                        /*
-                                         * Data reg in 2430, omap3 and
-                                         * omap4 is 8 bit wide
-                                         */
-                                        if (dev->flags &
-                                                 OMAP_I2C_FLAG_16BIT_DATA_REG) {
-                                                if (dev->buf_len) {
-                                                        *dev->buf++ = w >> 8;
-                                                        dev->buf_len--;
-                                                }
-                                        }
-                                } else {
-                                        if (stat & OMAP_I2C_STAT_RRDY)
-                                                dev_err(dev->dev,
-                                                        "RRDY IRQ while no data"
-                                                                " requested\n");
-                                        if (stat & OMAP_I2C_STAT_RDR)
-                                                dev_err(dev->dev,
-                                                        "RDR IRQ while no data"
-                                                                " requested\n");
-                                        break;
-                                }
-                        }
-                        omap_i2c_ack_stat(dev,
-                                stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR));
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_RDR);
+                        break;
+                }
+
+                if (stat & OMAP_I2C_STAT_RRDY) {
+                        u8 num_bytes = 1;
+
+                        if (dev->threshold)
+                                num_bytes = dev->threshold;
+
+                        omap_i2c_receive_data(dev, num_bytes, false);
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_RRDY);
                         continue;
                 }
-                if (stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR)) {
+
+                if (stat & OMAP_I2C_STAT_XDR) {
                         u8 num_bytes = 1;
-                        if (dev->fifo_size) {
-                                if (stat & OMAP_I2C_STAT_XRDY)
-                                        num_bytes = dev->fifo_size;
-                                else    /* read TXSTAT on XDR interrupt */
-                                        num_bytes = omap_i2c_read_reg(dev,
-                                                        OMAP_I2C_BUFSTAT_REG)
-                                                        & 0x3F;
-                        }
-                        while (num_bytes) {
-                                num_bytes--;
-                                w = 0;
-                                if (dev->buf_len) {
-                                        w = *dev->buf++;
-                                        dev->buf_len--;
-                                        /*
-                                         * Data reg in 2430, omap3 and
-                                         * omap4 is 8 bit wide
-                                         */
-                                        if (dev->flags &
-                                                 OMAP_I2C_FLAG_16BIT_DATA_REG) {
-                                                if (dev->buf_len) {
-                                                        w |= *dev->buf++ << 8;
-                                                        dev->buf_len--;
-                                                }
-                                        }
-                                } else {
-                                        if (stat & OMAP_I2C_STAT_XRDY)
-                                                dev_err(dev->dev,
-                                                        "XRDY IRQ while no "
-                                                        "data to send\n");
-                                        if (stat & OMAP_I2C_STAT_XDR)
-                                                dev_err(dev->dev,
-                                                        "XDR IRQ while no "
-                                                        "data to send\n");
-                                        break;
-                                }
-
-                                if ((dev->errata & I2C_OMAP_ERRATA_I462) &&
-                                    errata_omap3_i462(dev, &stat, &err))
-                                        goto complete;
-
-                                omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
-                        }
-                        omap_i2c_ack_stat(dev,
-                                stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
+                        int ret;
+
+                        if (dev->fifo_size)
+                                num_bytes = dev->buf_len;
+
+                        ret = omap_i2c_transmit_data(dev, num_bytes, true);
+                        if (ret < 0)
+                                break;
+
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_XDR);
+                        break;
+                }
+
+                if (stat & OMAP_I2C_STAT_XRDY) {
+                        u8 num_bytes = 1;
+                        int ret;
+
+                        if (dev->threshold)
+                                num_bytes = dev->threshold;
+
+                        ret = omap_i2c_transmit_data(dev, num_bytes, false);
+                        if (ret < 0)
+                                break;
+
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_XRDY);
                         continue;
                 }
+
                 if (stat & OMAP_I2C_STAT_ROVR) {
                         dev_err(dev->dev, "Receive overrun\n");
-                        dev->cmd_err |= OMAP_I2C_STAT_ROVR;
+                        err |= OMAP_I2C_STAT_ROVR;
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_ROVR);
+                        break;
                 }
+
                 if (stat & OMAP_I2C_STAT_XUDF) {
                         dev_err(dev->dev, "Transmit underflow\n");
-                        dev->cmd_err |= OMAP_I2C_STAT_XUDF;
+                        err |= OMAP_I2C_STAT_XUDF;
+                        omap_i2c_ack_stat(dev, OMAP_I2C_STAT_XUDF);
+                        break;
                 }
-        }
+        } while (stat);
+
+        omap_i2c_complete_cmd(dev, err);
+
+out:
+        spin_unlock_irqrestore(&dev->lock, flags);
 
-        return count ? IRQ_HANDLED : IRQ_NONE;
+        return IRQ_HANDLED;
 }
 
 static const struct i2c_algorithm omap_i2c_algo = {
@@ -943,12 +1057,12 @@ omap_i2c_probe(struct platform_device *pdev)
 {
         struct omap_i2c_dev     *dev;
         struct i2c_adapter      *adap;
-        struct resource         *mem, *irq, *ioarea;
+        struct resource         *mem;
         const struct omap_i2c_bus_platform_data *pdata =
                 pdev->dev.platform_data;
         struct device_node      *node = pdev->dev.of_node;
         const struct of_device_id *match;
-        irq_handler_t isr;
+        int irq;
         int r;
 
         /* NOTE: driver uses the static register mapping */
@@ -957,23 +1071,23 @@ omap_i2c_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "no mem resource?\n");
                 return -ENODEV;
         }
-        irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-        if (!irq) {
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
                 dev_err(&pdev->dev, "no irq resource?\n");
-                return -ENODEV;
+                return irq;
         }
 
-        ioarea = request_mem_region(mem->start, resource_size(mem),
-                        pdev->name);
-        if (!ioarea) {
-                dev_err(&pdev->dev, "I2C region already claimed\n");
-                return -EBUSY;
+        dev = devm_kzalloc(&pdev->dev, sizeof(struct omap_i2c_dev), GFP_KERNEL);
+        if (!dev) {
+                dev_err(&pdev->dev, "Menory allocation failed\n");
+                return -ENOMEM;
         }
 
-        dev = kzalloc(sizeof(struct omap_i2c_dev), GFP_KERNEL);
-        if (!dev) {
-                r = -ENOMEM;
-                goto err_release_region;
+        dev->base = devm_request_and_ioremap(&pdev->dev, mem);
+        if (!dev->base) {
+                dev_err(&pdev->dev, "I2C region already claimed\n");
+                return -ENOMEM;
         }
 
         match = of_match_device(of_match_ptr(omap_i2c_of_match), &pdev->dev);
@@ -990,17 +1104,13 @@ omap_i2c_probe(struct platform_device *pdev)
         } else if (pdata != NULL) {
                 dev->speed = pdata->clkrate;
                 dev->flags = pdata->flags;
-                dev->set_mpu_wkup_lat = pdata->set_mpu_wkup_lat;
                 dev->dtrev = pdata->rev;
         }
 
         dev->dev = &pdev->dev;
-        dev->irq = irq->start;
-        dev->base = ioremap(mem->start, resource_size(mem));
-        if (!dev->base) {
-                r = -ENOMEM;
-                goto err_free_mem;
-        }
+        dev->irq = irq;
+
+        spin_lock_init(&dev->lock);
 
         platform_set_drvdata(pdev, dev);
         init_completion(&dev->cmd_complete);
@@ -1013,6 +1123,9 @@ omap_i2c_probe(struct platform_device *pdev)
                 dev->regs = (u8 *)reg_map_ip_v1;
 
         pm_runtime_enable(dev->dev);
+        pm_runtime_set_autosuspend_delay(dev->dev, OMAP_I2C_PM_TIMEOUT);
+        pm_runtime_use_autosuspend(dev->dev);
+
         r = pm_runtime_get_sync(dev->dev);
         if (IS_ERR_VALUE(r))
                 goto err_free_mem;
@@ -1042,32 +1155,31 @@ omap_i2c_probe(struct platform_device *pdev)
 
                 dev->fifo_size = (dev->fifo_size / 2);
 
-                if (dev->rev >= OMAP_I2C_REV_ON_3630_4430)
-                        dev->b_hw = 0; /* Disable hardware fixes */
-                else
+                if (dev->rev < OMAP_I2C_REV_ON_3630_4430)
                         dev->b_hw = 1; /* Enable hardware fixes */
 
                 /* calculate wakeup latency constraint for MPU */
-                if (dev->set_mpu_wkup_lat != NULL)
-                        dev->latency = (1000000 * dev->fifo_size) /
-                                       (1000 * dev->speed / 8);
+                dev->latency = (1000000 * dev->fifo_size) /
+                               (1000 * dev->speed / 8);
         }
 
         /* reset ASAP, clearing any IRQs */
         omap_i2c_init(dev);
 
-        isr = (dev->rev < OMAP_I2C_OMAP1_REV_2) ? omap_i2c_omap1_isr :
-                                                                   omap_i2c_isr;
-        r = request_irq(dev->irq, isr, IRQF_NO_SUSPEND, pdev->name, dev);
+        if (dev->rev < OMAP_I2C_OMAP1_REV_2)
+                r = devm_request_irq(&pdev->dev, dev->irq, omap_i2c_omap1_isr,
+                                IRQF_NO_SUSPEND, pdev->name, dev);
+        else
+                r = devm_request_threaded_irq(&pdev->dev, dev->irq,
+                                omap_i2c_isr, omap_i2c_isr_thread,
+                                IRQF_NO_SUSPEND | IRQF_ONESHOT,
+                                pdev->name, dev);
 
         if (r) {
                 dev_err(dev->dev, "failure requesting irq %i\n", dev->irq);
                 goto err_unuse_clocks;
         }
 
-        dev_info(dev->dev, "bus %d rev%d.%d.%d at %d kHz\n", pdev->id,
-                 dev->dtrev, dev->rev >> 4, dev->rev & 0xf, dev->speed);
-
         adap = &dev->adapter;
         i2c_set_adapdata(adap, dev);
         adap->owner = THIS_MODULE;
@@ -1082,27 +1194,25 @@ omap_i2c_probe(struct platform_device *pdev)
         r = i2c_add_numbered_adapter(adap);
         if (r) {
                 dev_err(dev->dev, "failure adding adapter\n");
-                goto err_free_irq;
+                goto err_unuse_clocks;
         }
 
+        dev_info(dev->dev, "bus %d rev%d.%d.%d at %d kHz\n", adap->nr,
+                 dev->dtrev, dev->rev >> 4, dev->rev & 0xf, dev->speed);
+
         of_i2c_register_devices(adap);
 
-        pm_runtime_put(dev->dev);
+        pm_runtime_mark_last_busy(dev->dev);
+        pm_runtime_put_autosuspend(dev->dev);
 
         return 0;
 
-err_free_irq:
-        free_irq(dev->irq, dev);
 err_unuse_clocks:
         omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
         pm_runtime_put(dev->dev);
-        iounmap(dev->base);
         pm_runtime_disable(&pdev->dev);
 err_free_mem:
         platform_set_drvdata(pdev, NULL);
-        kfree(dev);
-err_release_region:
-        release_mem_region(mem->start, resource_size(mem));
 
         return r;
 }
@@ -1110,12 +1220,10 @@ err_release_region:
 static int __devexit omap_i2c_remove(struct platform_device *pdev)
 {
         struct omap_i2c_dev     *dev = platform_get_drvdata(pdev);
-        struct resource         *mem;
         int ret;
 
         platform_set_drvdata(pdev, NULL);
 
-        free_irq(dev->irq, dev);
         i2c_del_adapter(&dev->adapter);
         ret = pm_runtime_get_sync(&pdev->dev);
         if (IS_ERR_VALUE(ret))
@@ -1124,10 +1232,6 @@ static int __devexit omap_i2c_remove(struct platform_device *pdev)
         omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
         pm_runtime_put(&pdev->dev);
         pm_runtime_disable(&pdev->dev);
-        iounmap(dev->base);
-        kfree(dev);
-        mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        release_mem_region(mem->start, resource_size(mem));
         return 0;
 }
 
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
new file mode 100644
index 000000000000..f9399d163af2
--- /dev/null
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -0,0 +1,709 @@
+/*
+ *  drivers/i2c/busses/i2c-rcar.c
+ *
+ * Copyright (C) 2012 Renesas Solutions Corp.
+ * Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
+ *
+ * This file is based on the drivers/i2c/busses/i2c-sh7760.c
+ * (c) 2005-2008 MSC Vertriebsges.m.b.H, Manuel Lauss <mlau@msc-ge.com>
+ *
+ * This file used out-of-tree driver i2c-rcar.c
+ * Copyright (C) 2011-2012 Renesas Electronics Corporation
+ *
+ * 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
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/i2c/i2c-rcar.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+/* register offsets */
+#define ICSCR   0x00    /* slave ctrl */
+#define ICMCR   0x04    /* master ctrl */
+#define ICSSR   0x08    /* slave status */
+#define ICMSR   0x0C    /* master status */
+#define ICSIER  0x10    /* slave irq enable */
+#define ICMIER  0x14    /* master irq enable */
+#define ICCCR   0x18    /* clock dividers */
+#define ICSAR   0x1C    /* slave address */
+#define ICMAR   0x20    /* master address */
+#define ICRXTX  0x24    /* data port */
+
+/* ICMCR */
+#define MDBS    (1 << 7)        /* non-fifo mode switch */
+#define FSCL    (1 << 6)        /* override SCL pin */
+#define FSDA    (1 << 5)        /* override SDA pin */
+#define OBPC    (1 << 4)        /* override pins */
+#define MIE     (1 << 3)        /* master if enable */
+#define TSBE    (1 << 2)
+#define FSB     (1 << 1)        /* force stop bit */
+#define ESG     (1 << 0)        /* en startbit gen */
+
+/* ICMSR */
+#define MNR     (1 << 6)        /* nack received */
+#define MAL     (1 << 5)        /* arbitration lost */
+#define MST     (1 << 4)        /* sent a stop */
+#define MDE     (1 << 3)
+#define MDT     (1 << 2)
+#define MDR     (1 << 1)
+#define MAT     (1 << 0)        /* slave addr xfer done */
+
+/* ICMIE */
+#define MNRE    (1 << 6)        /* nack irq en */
+#define MALE    (1 << 5)        /* arblos irq en */
+#define MSTE    (1 << 4)        /* stop irq en */
+#define MDEE    (1 << 3)
+#define MDTE    (1 << 2)
+#define MDRE    (1 << 1)
+#define MATE    (1 << 0)        /* address sent irq en */
+
+
+enum {
+        RCAR_BUS_PHASE_ADDR,
+        RCAR_BUS_PHASE_DATA,
+        RCAR_BUS_PHASE_STOP,
+};
+
+enum {
+        RCAR_IRQ_CLOSE,
+        RCAR_IRQ_OPEN_FOR_SEND,
+        RCAR_IRQ_OPEN_FOR_RECV,
+        RCAR_IRQ_OPEN_FOR_STOP,
+};
+
+/*
+ * flags
+ */
+#define ID_LAST_MSG     (1 << 0)
+#define ID_IOERROR      (1 << 1)
+#define ID_DONE         (1 << 2)
+#define ID_ARBLOST      (1 << 3)
+#define ID_NACK         (1 << 4)
+
+struct rcar_i2c_priv {
+        void __iomem *io;
+        struct i2c_adapter adap;
+        struct i2c_msg  *msg;
+
+        spinlock_t lock;
+        wait_queue_head_t wait;
+
+        int pos;
+        int irq;
+        u32 icccr;
+        u32 flags;
+};
+
+#define rcar_i2c_priv_to_dev(p)         ((p)->adap.dev.parent)
+#define rcar_i2c_is_recv(p)             ((p)->msg->flags & I2C_M_RD)
+
+#define rcar_i2c_flags_set(p, f)        ((p)->flags |= (f))
+#define rcar_i2c_flags_has(p, f)        ((p)->flags & (f))
+
+#define LOOP_TIMEOUT    1024
+
+/*
+ *              basic functions
+ */
+static void rcar_i2c_write(struct rcar_i2c_priv *priv, int reg, u32 val)
+{
+        writel(val, priv->io + reg);
+}
+
+static u32 rcar_i2c_read(struct rcar_i2c_priv *priv, int reg)
+{
+        return readl(priv->io + reg);
+}
+
+static void rcar_i2c_init(struct rcar_i2c_priv *priv)
+{
+        /*
+         * reset slave mode.
+         * slave mode is not used on this driver
+         */
+        rcar_i2c_write(priv, ICSIER, 0);
+        rcar_i2c_write(priv, ICSAR, 0);
+        rcar_i2c_write(priv, ICSCR, 0);
+        rcar_i2c_write(priv, ICSSR, 0);
+
+        /* reset master mode */
+        rcar_i2c_write(priv, ICMIER, 0);
+        rcar_i2c_write(priv, ICMCR, 0);
+        rcar_i2c_write(priv, ICMSR, 0);
+        rcar_i2c_write(priv, ICMAR, 0);
+}
+
+static void rcar_i2c_irq_mask(struct rcar_i2c_priv *priv, int open)
+{
+        u32 val = MNRE | MALE | MSTE | MATE; /* default */
+
+        switch (open) {
+        case RCAR_IRQ_OPEN_FOR_SEND:
+                val |= MDEE; /* default + send */
+                break;
+        case RCAR_IRQ_OPEN_FOR_RECV:
+                val |= MDRE; /* default + read */
+                break;
+        case RCAR_IRQ_OPEN_FOR_STOP:
+                val = MSTE; /* stop irq only */
+                break;
+        case RCAR_IRQ_CLOSE:
+        default:
+                val = 0; /* all close */
+                break;
+        }
+        rcar_i2c_write(priv, ICMIER, val);
+}
+
+static void rcar_i2c_set_addr(struct rcar_i2c_priv *priv, u32 recv)
+{
+        rcar_i2c_write(priv, ICMAR, (priv->msg->addr << 1) | recv);
+}
+
+/*
+ *              bus control functions
+ */
+static int rcar_i2c_bus_barrier(struct rcar_i2c_priv *priv)
+{
+        int i;
+
+        for (i = 0; i < LOOP_TIMEOUT; i++) {
+                /* make sure that bus is not busy */
+                if (!(rcar_i2c_read(priv, ICMCR) & FSDA))
+                        return 0;
+                udelay(1);
+        }
+
+        return -EBUSY;
+}
+
+static void rcar_i2c_bus_phase(struct rcar_i2c_priv *priv, int phase)
+{
+        switch (phase) {
+        case RCAR_BUS_PHASE_ADDR:
+                rcar_i2c_write(priv, ICMCR, MDBS | MIE | ESG);
+                break;
+        case RCAR_BUS_PHASE_DATA:
+                rcar_i2c_write(priv, ICMCR, MDBS | MIE);
+                break;
+        case RCAR_BUS_PHASE_STOP:
+                rcar_i2c_write(priv, ICMCR, MDBS | MIE | FSB);
+                break;
+        }
+}
+
+/*
+ *              clock function
+ */
+static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
+                                    u32 bus_speed,
+                                    struct device *dev)
+{
+        struct clk *clkp = clk_get(NULL, "peripheral_clk");
+        u32 scgd, cdf;
+        u32 round, ick;
+        u32 scl;
+
+        if (!clkp) {
+                dev_err(dev, "there is no peripheral_clk\n");
+                return -EIO;
+        }
+
+        /*
+         * calculate SCL clock
+         * see
+         *      ICCCR
+         *
+         * ick  = clkp / (1 + CDF)
+         * SCL  = ick / (20 + SCGD * 8 + F[(ticf + tr + intd) * ick])
+         *
+         * ick  : I2C internal clock < 20 MHz
+         * ticf : I2C SCL falling time  =  35 ns here
+         * tr   : I2C SCL rising  time  = 200 ns here
+         * intd : LSI internal delay    =  50 ns here
+         * clkp : peripheral_clk
+         * F[]  : integer up-valuation
+         */
+        for (cdf = 0; cdf < 4; cdf++) {
+                ick = clk_get_rate(clkp) / (1 + cdf);
+                if (ick < 20000000)
+                        goto ick_find;
+        }
+        dev_err(dev, "there is no best CDF\n");
+        return -EIO;
+
+ick_find:
+        /*
+         * it is impossible to calculate large scale
+         * number on u32. separate it
+         *
+         * F[(ticf + tr + intd) * ick]
+         *  = F[(35 + 200 + 50)ns * ick]
+         *  = F[285 * ick / 1000000000]
+         *  = F[(ick / 1000000) * 285 / 1000]
+         */
+        round = (ick + 500000) / 1000000 * 285;
+        round = (round + 500) / 1000;
+
+        /*
+         * SCL  = ick / (20 + SCGD * 8 + F[(ticf + tr + intd) * ick])
+         *
+         * Calculation result (= SCL) should be less than
+         * bus_speed for hardware safety
+         */
+        for (scgd = 0; scgd < 0x40; scgd++) {
+                scl = ick / (20 + (scgd * 8) + round);
+                if (scl <= bus_speed)
+                        goto scgd_find;
+        }
+        dev_err(dev, "it is impossible to calculate best SCL\n");
+        return -EIO;
+
+scgd_find:
+        dev_dbg(dev, "clk %d/%d(%lu), round %u, CDF:0x%x, SCGD: 0x%x\n",
+                scl, bus_speed, clk_get_rate(clkp), round, cdf, scgd);
+
+        /*
+         * keep icccr value
+         */
+        priv->icccr = (scgd << 2 | cdf);
+
+        return 0;
+}
+
+static void rcar_i2c_clock_start(struct rcar_i2c_priv *priv)
+{
+        rcar_i2c_write(priv, ICCCR, priv->icccr);
+}
+
+/*
+ *              status functions
+ */
+static u32 rcar_i2c_status_get(struct rcar_i2c_priv *priv)
+{
+        return rcar_i2c_read(priv, ICMSR);
+}
+
+#define rcar_i2c_status_clear(priv) rcar_i2c_status_bit_clear(priv, 0xffffffff)
+static void rcar_i2c_status_bit_clear(struct rcar_i2c_priv *priv, u32 bit)
+{
+        rcar_i2c_write(priv, ICMSR, ~bit);
+}
+
+/*
+ *              recv/send functions
+ */
+static int rcar_i2c_recv(struct rcar_i2c_priv *priv)
+{
+        rcar_i2c_set_addr(priv, 1);
+        rcar_i2c_status_clear(priv);
+        rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_ADDR);
+        rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_RECV);
+
+        return 0;
+}
+
+static int rcar_i2c_send(struct rcar_i2c_priv *priv)
+{
+        int ret;
+
+        /*
+         * It should check bus status when send case
+         */
+        ret = rcar_i2c_bus_barrier(priv);
+        if (ret < 0)
+                return ret;
+
+        rcar_i2c_set_addr(priv, 0);
+        rcar_i2c_status_clear(priv);
+        rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_ADDR);
+        rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_SEND);
+
+        return 0;
+}
+
+#define rcar_i2c_send_restart(priv) rcar_i2c_status_bit_clear(priv, (MAT | MDE))
+#define rcar_i2c_recv_restart(priv) rcar_i2c_status_bit_clear(priv, (MAT | MDR))
+
+/*
+ *              interrupt functions
+ */
+static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
+{
+        struct i2c_msg *msg = priv->msg;
+
+        /*
+         * FIXME
+         * sometimes, unknown interrupt happened.
+         * Do nothing
+         */
+        if (!(msr & MDE))
+                return 0;
+
+        /*
+         * If address transfer phase finished,
+         * goto data phase.
+         */
+        if (msr & MAT)
+                rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_DATA);
+
+        if (priv->pos < msg->len) {
+                /*
+                 * Prepare next data to ICRXTX register.
+                 * This data will go to _SHIFT_ register.
+                 *
+                 *    *
+                 * [ICRXTX] -> [SHIFT] -> [I2C bus]
+                 */
+                rcar_i2c_write(priv, ICRXTX, msg->buf[priv->pos]);
+                priv->pos++;
+
+        } else {
+                /*
+                 * The last data was pushed to ICRXTX on _PREV_ empty irq.
+                 * It is on _SHIFT_ register, and will sent to I2C bus.
+                 *
+                 *                *
+                 * [ICRXTX] -> [SHIFT] -> [I2C bus]
+                 */
+
+                if (priv->flags & ID_LAST_MSG)
+                        /*
+                         * If current msg is the _LAST_ msg,
+                         * prepare stop condition here.
+                         * ID_DONE will be set on STOP irq.
+                         */
+                        rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+                else
+                        /*
+                         * If current msg is _NOT_ last msg,
+                         * it doesn't call stop phase.
+                         * thus, there is no STOP irq.
+                         * return ID_DONE here.
+                         */
+                        return ID_DONE;
+        }
+
+        rcar_i2c_send_restart(priv);
+
+        return 0;
+}
+
+static int rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
+{
+        struct i2c_msg *msg = priv->msg;
+
+        /*
+         * FIXME
+         * sometimes, unknown interrupt happened.
+         * Do nothing
+         */
+        if (!(msr & MDR))
+                return 0;
+
+        if (msr & MAT) {
+                /*
+                 * Address transfer phase finished,
+                 * but, there is no data at this point.
+                 * Do nothing.
+                 */
+        } else if (priv->pos < msg->len) {
+                /*
+                 * get received data
+                 */
+                msg->buf[priv->pos] = rcar_i2c_read(priv, ICRXTX);
+                priv->pos++;
+        }
+
+        /*
+         * If next received data is the _LAST_,
+         * go to STOP phase,
+         * otherwise, go to DATA phase.
+         */
+        if (priv->pos + 1 >= msg->len)
+                rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+        else
+                rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_DATA);
+
+        rcar_i2c_recv_restart(priv);
+
+        return 0;
+}
+
+static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
+{
+        struct rcar_i2c_priv *priv = ptr;
+        struct device *dev = rcar_i2c_priv_to_dev(priv);
+        u32 msr;
+
+        /*-------------- spin lock -----------------*/
+        spin_lock(&priv->lock);
+
+        msr = rcar_i2c_status_get(priv);
+
+        /*
+         * Arbitration lost
+         */
+        if (msr & MAL) {
+                /*
+                 * CAUTION
+                 *
+                 * When arbitration lost, device become _slave_ mode.
+                 */
+                dev_dbg(dev, "Arbitration Lost\n");
+                rcar_i2c_flags_set(priv, (ID_DONE | ID_ARBLOST));
+                goto out;
+        }
+
+        /*
+         * Stop
+         */
+        if (msr & MST) {
+                dev_dbg(dev, "Stop\n");
+                rcar_i2c_flags_set(priv, ID_DONE);
+                goto out;
+        }
+
+        /*
+         * Nack
+         */
+        if (msr & MNR) {
+                dev_dbg(dev, "Nack\n");
+
+                /* go to stop phase */
+                rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+                rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_STOP);
+                rcar_i2c_flags_set(priv, ID_NACK);
+                goto out;
+        }
+
+        /*
+         * recv/send
+         */
+        if (rcar_i2c_is_recv(priv))
+                rcar_i2c_flags_set(priv, rcar_i2c_irq_recv(priv, msr));
+        else
+                rcar_i2c_flags_set(priv, rcar_i2c_irq_send(priv, msr));
+
+out:
+        if (rcar_i2c_flags_has(priv, ID_DONE)) {
+                rcar_i2c_irq_mask(priv, RCAR_IRQ_CLOSE);
+                rcar_i2c_status_clear(priv);
+                wake_up(&priv->wait);
+        }
+
+        spin_unlock(&priv->lock);
+        /*-------------- spin unlock -----------------*/
+
+        return IRQ_HANDLED;
+}
+
+static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
+                                struct i2c_msg *msgs,
+                                int num)
+{
+        struct rcar_i2c_priv *priv = i2c_get_adapdata(adap);
+        struct device *dev = rcar_i2c_priv_to_dev(priv);
+        unsigned long flags;
+        int i, ret, timeout;
+
+        pm_runtime_get_sync(dev);
+
+        /*-------------- spin lock -----------------*/
+        spin_lock_irqsave(&priv->lock, flags);
+
+        rcar_i2c_init(priv);
+        rcar_i2c_clock_start(priv);
+
+        spin_unlock_irqrestore(&priv->lock, flags);
+        /*-------------- spin unlock -----------------*/
+
+        ret = -EINVAL;
+        for (i = 0; i < num; i++) {
+                /*-------------- spin lock -----------------*/
+                spin_lock_irqsave(&priv->lock, flags);
+
+                /* init each data */
+                priv->msg       = &msgs[i];
+                priv->pos       = 0;
+                priv->flags     = 0;
+                if (priv->msg == &msgs[num - 1])
+                        rcar_i2c_flags_set(priv, ID_LAST_MSG);
+
+                /* start send/recv */
+                if (rcar_i2c_is_recv(priv))
+                        ret = rcar_i2c_recv(priv);
+                else
+                        ret = rcar_i2c_send(priv);
+
+                spin_unlock_irqrestore(&priv->lock, flags);
+                /*-------------- spin unlock -----------------*/
+
+                if (ret < 0)
+                        break;
+
+                /*
+                 * wait result
+                 */
+                timeout = wait_event_timeout(priv->wait,
+                                             rcar_i2c_flags_has(priv, ID_DONE),
+                                             5 * HZ);
+                if (!timeout) {
+                        ret = -ETIMEDOUT;
+                        break;
+                }
+
+                /*
+                 * error handling
+                 */
+                if (rcar_i2c_flags_has(priv, ID_NACK)) {
+                        ret = -EREMOTEIO;
+                        break;
+                }
+
+                if (rcar_i2c_flags_has(priv, ID_ARBLOST)) {
+                        ret = -EAGAIN;
+                        break;
+                }
+
+                if (rcar_i2c_flags_has(priv, ID_IOERROR)) {
+                        ret = -EIO;
+                        break;
+                }
+
+                ret = i + 1; /* The number of transfer */
+        }
+
+        pm_runtime_put(dev);
+
+        if (ret < 0)
+                dev_err(dev, "error %d : %x\n", ret, priv->flags);
+
+        return ret;
+}
+
+static u32 rcar_i2c_func(struct i2c_adapter *adap)
+{
+        return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm rcar_i2c_algo = {
+        .master_xfer    = rcar_i2c_master_xfer,
+        .functionality  = rcar_i2c_func,
+};
+
+static int __devinit rcar_i2c_probe(struct platform_device *pdev)
+{
+        struct i2c_rcar_platform_data *pdata = pdev->dev.platform_data;
+        struct rcar_i2c_priv *priv;
+        struct i2c_adapter *adap;
+        struct resource *res;
+        struct device *dev = &pdev->dev;
+        u32 bus_speed;
+        int ret;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(dev, "no mmio resources\n");
+                return -ENODEV;
+        }
+
+        priv = devm_kzalloc(dev, sizeof(struct rcar_i2c_priv), GFP_KERNEL);
+        if (!priv) {
+                dev_err(dev, "no mem for private data\n");
+                return -ENOMEM;
+        }
+
+        bus_speed = 100000; /* default 100 kHz */
+        if (pdata && pdata->bus_speed)
+                bus_speed = pdata->bus_speed;
+        ret = rcar_i2c_clock_calculate(priv, bus_speed, dev);
+        if (ret < 0)
+                return ret;
+
+        priv->io = devm_ioremap(dev, res->start, resource_size(res));
+        if (!priv->io) {
+                dev_err(dev, "cannot ioremap\n");
+                return -ENODEV;
+        }
+
+        priv->irq = platform_get_irq(pdev, 0);
+        init_waitqueue_head(&priv->wait);
+        spin_lock_init(&priv->lock);
+
+        adap                    = &priv->adap;
+        adap->nr                = pdev->id;
+        adap->algo              = &rcar_i2c_algo;
+        adap->class             = I2C_CLASS_HWMON | I2C_CLASS_SPD;
+        adap->retries           = 3;
+        adap->dev.parent        = dev;
+        i2c_set_adapdata(adap, priv);
+        strlcpy(adap->name, pdev->name, sizeof(adap->name));
+
+        ret = devm_request_irq(dev, priv->irq, rcar_i2c_irq, 0,
+                               dev_name(dev), priv);
+        if (ret < 0) {
+                dev_err(dev, "cannot get irq %d\n", priv->irq);
+                return ret;
+        }
+
+        ret = i2c_add_numbered_adapter(adap);
+        if (ret < 0) {
+                dev_err(dev, "reg adap failed: %d\n", ret);
+                return ret;
+        }
+
+        pm_runtime_enable(dev);
+        platform_set_drvdata(pdev, priv);
+
+        dev_info(dev, "probed\n");
+
+        return 0;
+}
+
+static int __devexit rcar_i2c_remove(struct platform_device *pdev)
+{
+        struct rcar_i2c_priv *priv = platform_get_drvdata(pdev);
+        struct device *dev = &pdev->dev;
+
+        i2c_del_adapter(&priv->adap);
+        pm_runtime_disable(dev);
+
+        return 0;
+}
+
+static struct platform_driver rcar_i2c_drv = {
+        .driver = {
+                .name   = "i2c-rcar",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = rcar_i2c_probe,
+        .remove         = __devexit_p(rcar_i2c_remove),
+};
+
+module_platform_driver(rcar_i2c_drv);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Renesas R-Car I2C bus driver");
+MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 4d07dea9bca9..3e0335f1fc60 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -601,14 +601,14 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
         int ret;
 
         pm_runtime_get_sync(&adap->dev);
-        clk_enable(i2c->clk);
+        clk_prepare_enable(i2c->clk);
 
         for (retry = 0; retry < adap->retries; retry++) {
 
                 ret = s3c24xx_i2c_doxfer(i2c, msgs, num);
 
                 if (ret != -EAGAIN) {
-                        clk_disable(i2c->clk);
+                        clk_disable_unprepare(i2c->clk);
                         pm_runtime_put(&adap->dev);
                         return ret;
                 }
@@ -618,7 +618,7 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
                 udelay(100);
         }
 
-        clk_disable(i2c->clk);
+        clk_disable_unprepare(i2c->clk);
         pm_runtime_put(&adap->dev);
         return -EREMOTEIO;
 }
@@ -977,7 +977,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 
         dev_dbg(&pdev->dev, "clock source %p\n", i2c->clk);
 
-        clk_enable(i2c->clk);
+        clk_prepare_enable(i2c->clk);
 
         /* map the registers */
 
@@ -1065,7 +1065,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
         pm_runtime_enable(&i2c->adap.dev);
 
         dev_info(&pdev->dev, "%s: S3C I2C adapter\n", dev_name(&i2c->adap.dev));
-        clk_disable(i2c->clk);
+        clk_disable_unprepare(i2c->clk);
         return 0;
 
  err_cpufreq:
@@ -1082,7 +1082,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
         kfree(i2c->ioarea);
 
  err_clk:
-        clk_disable(i2c->clk);
+        clk_disable_unprepare(i2c->clk);
         clk_put(i2c->clk);
 
  err_noclk:
@@ -1106,7 +1106,7 @@ static int s3c24xx_i2c_remove(struct platform_device *pdev)
         i2c_del_adapter(&i2c->adap);
         free_irq(i2c->irq, i2c);
 
-        clk_disable(i2c->clk);
+        clk_disable_unprepare(i2c->clk);
         clk_put(i2c->clk);
 
         iounmap(i2c->regs);
@@ -1135,9 +1135,9 @@ static int s3c24xx_i2c_resume(struct device *dev)
         struct s3c24xx_i2c *i2c = platform_get_drvdata(pdev);
 
         i2c->suspended = 0;
-        clk_enable(i2c->clk);
+        clk_prepare_enable(i2c->clk);
         s3c24xx_i2c_init(i2c);
-        clk_disable(i2c->clk);
+        clk_disable_unprepare(i2c->clk);
 
         return 0;
 }
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index f981ac4e6783..dcea77bf6f50 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -742,7 +742,7 @@ static int __devinit tegra_i2c_probe(struct platform_device *pdev)
         }
 
         ret = devm_request_irq(&pdev->dev, i2c_dev->irq,
-                        tegra_i2c_isr, 0, pdev->name, i2c_dev);
+                        tegra_i2c_isr, 0, dev_name(&pdev->dev), i2c_dev);
         if (ret) {
                 dev_err(&pdev->dev, "Failed to request irq %i\n", i2c_dev->irq);
                 return ret;
diff --git a/drivers/i2c/busses/i2c-stub.c b/drivers/i2c/i2c-stub.c
index b1b3447942c9..d0a9c590c3cd 100644
--- a/drivers/i2c/busses/i2c-stub.c
+++ b/drivers/i2c/i2c-stub.c
@@ -2,7 +2,7 @@
     i2c-stub.c - I2C/SMBus chip emulator
 
     Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
-    Copyright (C) 2007 Jean Delvare <khali@linux-fr.org>
+    Copyright (C) 2007, 2012 Jean Delvare <khali@linux-fr.org>
 
     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
@@ -51,8 +51,8 @@ struct stub_chip {
 static struct stub_chip *stub_chips;
 
 /* Return negative errno on error. */
-static s32 stub_xfer(struct i2c_adapter * adap, u16 addr, unsigned short flags,
-        char read_write, u8 command, int size, union i2c_smbus_data * data)
+static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags,
+        char read_write, u8 command, int size, union i2c_smbus_data *data)
 {
         s32 ret;
         int i, len;
@@ -78,14 +78,14 @@ static s32 stub_xfer(struct i2c_adapter * adap, u16 addr, unsigned short flags,
         case I2C_SMBUS_BYTE:
                 if (read_write == I2C_SMBUS_WRITE) {
                         chip->pointer = command;
-                        dev_dbg(&adap->dev, "smbus byte - addr 0x%02x, "
-                                        "wrote 0x%02x.\n",
-                                        addr, command);
+                        dev_dbg(&adap->dev,
+                                "smbus byte - addr 0x%02x, wrote 0x%02x.\n",
+                                addr, command);
                 } else {
                         data->byte = chip->words[chip->pointer++] & 0xff;
-                        dev_dbg(&adap->dev, "smbus byte - addr 0x%02x, "
-                                        "read  0x%02x.\n",
-                                        addr, data->byte);
+                        dev_dbg(&adap->dev,
+                                "smbus byte - addr 0x%02x, read  0x%02x.\n",
+                                addr, data->byte);
                 }
 
                 ret = 0;
@@ -95,14 +95,14 @@ static s32 stub_xfer(struct i2c_adapter * adap, u16 addr, unsigned short flags,
                 if (read_write == I2C_SMBUS_WRITE) {
                         chip->words[command] &= 0xff00;
                         chip->words[command] |= data->byte;
-                        dev_dbg(&adap->dev, "smbus byte data - addr 0x%02x, "
-                                        "wrote 0x%02x at 0x%02x.\n",
-                                        addr, data->byte, command);
+                        dev_dbg(&adap->dev,
+                                "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
+                                addr, data->byte, command);
                 } else {
                         data->byte = chip->words[command] & 0xff;
-                        dev_dbg(&adap->dev, "smbus byte data - addr 0x%02x, "
-                                        "read  0x%02x at 0x%02x.\n",
-                                        addr, data->byte, command);
+                        dev_dbg(&adap->dev,
+                                "smbus byte data - addr 0x%02x, read  0x%02x at 0x%02x.\n",
+                                addr, data->byte, command);
                 }
                 chip->pointer = command + 1;
 
@@ -112,14 +112,14 @@ static s32 stub_xfer(struct i2c_adapter * adap, u16 addr, unsigned short flags,
         case I2C_SMBUS_WORD_DATA:
                 if (read_write == I2C_SMBUS_WRITE) {
                         chip->words[command] = data->word;
-                        dev_dbg(&adap->dev, "smbus word data - addr 0x%02x, "
-                                        "wrote 0x%04x at 0x%02x.\n",
-                                        addr, data->word, command);
+                        dev_dbg(&adap->dev,
+                                "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",
+                                addr, data->word, command);
                 } else {
                         data->word = chip->words[command];
-                        dev_dbg(&adap->dev, "smbus word data - addr 0x%02x, "
-                                        "read  0x%04x at 0x%02x.\n",
-                                        addr, data->word, command);
+                        dev_dbg(&adap->dev,
+                                "smbus word data - addr 0x%02x, read  0x%04x at 0x%02x.\n",
+                                addr, data->word, command);
                 }
 
                 ret = 0;
@@ -132,17 +132,17 @@ static s32 stub_xfer(struct i2c_adapter * adap, u16 addr, unsigned short flags,
                                 chip->words[command + i] &= 0xff00;
                                 chip->words[command + i] |= data->block[1 + i];
                         }
-                        dev_dbg(&adap->dev, "i2c block data - addr 0x%02x, "
-                                        "wrote %d bytes at 0x%02x.\n",
-                                        addr, len, command);
+                        dev_dbg(&adap->dev,
+                                "i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
+                                addr, len, command);
                 } else {
                         for (i = 0; i < len; i++) {
                                 data->block[1 + i] =
                                         chip->words[command + i] & 0xff;
                         }
-                        dev_dbg(&adap->dev, "i2c block data - addr 0x%02x, "
-                                        "read  %d bytes at 0x%02x.\n",
-                                        addr, len, command);
+                        dev_dbg(&adap->dev,
+                                "i2c block data - addr 0x%02x, read  %d bytes at 0x%02x.\n",
+                                addr, len, command);
                 }
 
                 ret = 0;
@@ -179,25 +179,24 @@ static int __init i2c_stub_init(void)
         int i, ret;
 
         if (!chip_addr[0]) {
-                printk(KERN_ERR "i2c-stub: Please specify a chip address\n");
+                pr_err("i2c-stub: Please specify a chip address\n");
                 return -ENODEV;
         }
 
         for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
                 if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) {
-                        printk(KERN_ERR "i2c-stub: Invalid chip address "
-                               "0x%02x\n", chip_addr[i]);
+                        pr_err("i2c-stub: Invalid chip address 0x%02x\n",
+                               chip_addr[i]);
                         return -EINVAL;
                 }
 
-                printk(KERN_INFO "i2c-stub: Virtual chip at 0x%02x\n",
-                       chip_addr[i]);
+                pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr[i]);
         }
 
         /* Allocate memory for all chips at once */
         stub_chips = kzalloc(i * sizeof(struct stub_chip), GFP_KERNEL);
         if (!stub_chips) {
-                printk(KERN_ERR "i2c-stub: Out of memory\n");
+                pr_err("i2c-stub: Out of memory\n");
                 return -ENOMEM;
         }
 
@@ -219,4 +218,3 @@ MODULE_LICENSE("GPL");
 
 module_init(i2c_stub_init);
 module_exit(i2c_stub_exit);
-
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index 6e3f143fc71d..fc937aca71fb 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -62,7 +62,6 @@ source "drivers/iio/frequency/Kconfig"
 source "drivers/iio/dac/Kconfig"
 source "drivers/iio/common/Kconfig"
 source "drivers/iio/gyro/Kconfig"
-source "drivers/iio/light/Kconfig"
 source "drivers/iio/magnetometer/Kconfig"
 
 endif # IIO
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index f7fa3c0867b4..761f2b65ac52 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -18,5 +18,4 @@ obj-y += frequency/
 obj-y += dac/
 obj-y += common/
 obj-y += gyro/
-obj-y += light/
 obj-y += magnetometer/
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 1983adc19243..a7568c34a1aa 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -3498,7 +3498,8 @@ out:
 }
 
 static const struct ibnl_client_cbs cma_cb_table[] = {
-        [RDMA_NL_RDMA_CM_ID_STATS] = { .dump = cma_get_id_stats },
+        [RDMA_NL_RDMA_CM_ID_STATS] = { .dump = cma_get_id_stats,
+                                       .module = THIS_MODULE },
 };
 
 static int __init cma_init(void)
diff --git a/drivers/infiniband/core/netlink.c b/drivers/infiniband/core/netlink.c
index fe10a949aef9..da06abde9e0d 100644
--- a/drivers/infiniband/core/netlink.c
+++ b/drivers/infiniband/core/netlink.c
@@ -154,6 +154,7 @@ static int ibnl_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
                         {
                                 struct netlink_dump_control c = {
                                         .dump = client->cb_table[op].dump,
+                                        .module = client->cb_table[op].module,
                                 };
                                 return netlink_dump_start(nls, skb, nlh, &c);
                         }
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index 57e07c61ace2..afd81790ab3c 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -468,7 +468,7 @@ struct ib_mr *c4iw_register_phys_mem(struct ib_pd *pd,
         ret = alloc_pbl(mhp, npages);
         if (ret) {
                 kfree(page_list);
-                goto err_pbl;
+                goto err;
         }
 
         ret = write_pbl(&mhp->rhp->rdev, page_list, mhp->attr.pbl_addr,
diff --git a/drivers/infiniband/hw/mlx4/alias_GUID.c b/drivers/infiniband/hw/mlx4/alias_GUID.c
index d2fb38d43571..2f215b93db6b 100644
--- a/drivers/infiniband/hw/mlx4/alias_GUID.c
+++ b/drivers/infiniband/hw/mlx4/alias_GUID.c
@@ -107,7 +107,7 @@ static __be64 get_cached_alias_guid(struct mlx4_ib_dev *dev, int port, int index
 {
         if (index >= NUM_ALIAS_GUID_PER_PORT) {
                 pr_err("%s: ERROR: asked for index:%d\n", __func__, index);
-                return  (__force __be64) ((u64) 0xFFFFFFFFFFFFFFFFUL);
+                return (__force __be64) -1;
         }
         return *(__be64 *)&dev->sriov.demux[port - 1].guid_cache[index];
 }
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index 21a794152d15..0a903c129f0a 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -409,38 +409,45 @@ int mlx4_ib_find_real_gid(struct ib_device *ibdev, u8 port, __be64 guid)
 }
 
 
-static int get_pkey_phys_indices(struct mlx4_ib_dev *ibdev, u8 port, u8 ph_pkey_ix,
-                                 u8 *full_pk_ix, u8 *partial_pk_ix,
-                                 int *is_full_member)
+static int find_slave_port_pkey_ix(struct mlx4_ib_dev *dev, int slave,
+                                   u8 port, u16 pkey, u16 *ix)
 {
-        u16 search_pkey;
-        int fm;
-        int err = 0;
-        u16 pk;
+        int i, ret;
+        u8 unassigned_pkey_ix, pkey_ix, partial_ix = 0xFF;
+        u16 slot_pkey;
 
-        err = ib_get_cached_pkey(&ibdev->ib_dev, port, ph_pkey_ix, &search_pkey);
-        if (err)
-                return err;
+        if (slave == mlx4_master_func_num(dev->dev))
+                return ib_find_cached_pkey(&dev->ib_dev, port, pkey, ix);
 
-        fm = (search_pkey & 0x8000) ? 1 : 0;
-        if (fm) {
-                *full_pk_ix = ph_pkey_ix;
-                search_pkey &= 0x7FFF;
-        } else {
-                *partial_pk_ix = ph_pkey_ix;
-                search_pkey |= 0x8000;
-        }
+        unassigned_pkey_ix = dev->dev->phys_caps.pkey_phys_table_len[port] - 1;
 
-        if (ib_find_exact_cached_pkey(&ibdev->ib_dev, port, search_pkey, &pk))
-                pk = 0xFFFF;
+        for (i = 0; i < dev->dev->caps.pkey_table_len[port]; i++) {
+                if (dev->pkeys.virt2phys_pkey[slave][port - 1][i] == unassigned_pkey_ix)
+                        continue;
 
-        if (fm)
-                *partial_pk_ix = (pk & 0xFF);
-        else
-                *full_pk_ix = (pk & 0xFF);
+                pkey_ix = dev->pkeys.virt2phys_pkey[slave][port - 1][i];
 
-        *is_full_member = fm;
-        return err;
+                ret = ib_get_cached_pkey(&dev->ib_dev, port, pkey_ix, &slot_pkey);
+                if (ret)
+                        continue;
+                if ((slot_pkey & 0x7FFF) == (pkey & 0x7FFF)) {
+                        if (slot_pkey & 0x8000) {
+                                *ix = (u16) pkey_ix;
+                                return 0;
+                        } else {
+                                /* take first partial pkey index found */
+                                if (partial_ix == 0xFF)
+                                        partial_ix = pkey_ix;
+                        }
+                }
+        }
+
+        if (partial_ix < 0xFF) {
+                *ix = (u16) partial_ix;
+                return 0;
+        }
+
+        return -EINVAL;
 }
 
 int mlx4_ib_send_to_slave(struct mlx4_ib_dev *dev, int slave, u8 port,
@@ -458,10 +465,8 @@ int mlx4_ib_send_to_slave(struct mlx4_ib_dev *dev, int slave, u8 port,
         unsigned tun_tx_ix = 0;
         int dqpn;
         int ret = 0;
-        int i;
-        int is_full_member = 0;
         u16 tun_pkey_ix;
-        u8 ph_pkey_ix, full_pk_ix = 0, partial_pk_ix = 0;
+        u16 cached_pkey;
 
         if (dest_qpt > IB_QPT_GSI)
                 return -EINVAL;
@@ -481,27 +486,17 @@ int mlx4_ib_send_to_slave(struct mlx4_ib_dev *dev, int slave, u8 port,
         else
                 tun_qp = &tun_ctx->qp[1];
 
-        /* compute pkey index for slave */
-        /* get physical pkey -- virtualized Dom0 pkey to phys*/
+        /* compute P_Key index to put in tunnel header for slave */
         if (dest_qpt) {
-                ph_pkey_ix =
-                        dev->pkeys.virt2phys_pkey[mlx4_master_func_num(dev->dev)][port - 1][wc->pkey_index];
-
-                /* now, translate this to the slave pkey index */
-                ret = get_pkey_phys_indices(dev, port, ph_pkey_ix, &full_pk_ix,
-                                            &partial_pk_ix, &is_full_member);
+                u16 pkey_ix;
+                ret = ib_get_cached_pkey(&dev->ib_dev, port, wc->pkey_index, &cached_pkey);
                 if (ret)
                         return -EINVAL;
 
-                for (i = 0; i < dev->dev->caps.pkey_table_len[port]; i++) {
-                        if ((dev->pkeys.virt2phys_pkey[slave][port - 1][i] == full_pk_ix) ||
-                            (is_full_member &&
-                             (dev->pkeys.virt2phys_pkey[slave][port - 1][i] == partial_pk_ix)))
-                                break;
-                }
-                if (i == dev->dev->caps.pkey_table_len[port])
+                ret = find_slave_port_pkey_ix(dev, slave, port, cached_pkey, &pkey_ix);
+                if (ret)
                         return -EINVAL;
-                tun_pkey_ix = i;
+                tun_pkey_ix = pkey_ix;
         } else
                 tun_pkey_ix = dev->pkeys.virt2phys_pkey[slave][port - 1][0];
 
diff --git a/drivers/infiniband/hw/mlx4/mcg.c b/drivers/infiniband/hw/mlx4/mcg.c
index 3c3b54c3fdd9..25b2cdff00f8 100644
--- a/drivers/infiniband/hw/mlx4/mcg.c
+++ b/drivers/infiniband/hw/mlx4/mcg.c
@@ -233,7 +233,8 @@ static int send_mad_to_slave(int slave, struct mlx4_ib_demux_ctx *ctx,
 
         ib_query_ah(dev->sm_ah[ctx->port - 1], &ah_attr);
 
-        wc.pkey_index = 0;
+        if (ib_find_cached_pkey(&dev->ib_dev, ctx->port, IB_DEFAULT_PKEY_FULL, &wc.pkey_index))
+                return -EINVAL;
         wc.sl = 0;
         wc.dlid_path_bits = 0;
         wc.port_num = ctx->port;
@@ -1074,10 +1075,6 @@ static void _mlx4_ib_mcg_port_cleanup(struct mlx4_ib_demux_ctx *ctx, int destroy
         unsigned long end;
         int count;
 
-        if (ctx->flushing)
-                return;
-
-        ctx->flushing = 1;
         for (i = 0; i < MAX_VFS; ++i)
                 clean_vf_mcast(ctx, i);
 
@@ -1107,9 +1104,6 @@ static void _mlx4_ib_mcg_port_cleanup(struct mlx4_ib_demux_ctx *ctx, int destroy
                 force_clean_group(group);
         }
         mutex_unlock(&ctx->mcg_table_lock);
-
-        if (!destroy_wq)
-                ctx->flushing = 0;
 }
 
 struct clean_work {
@@ -1123,6 +1117,7 @@ static void mcg_clean_task(struct work_struct *work)
         struct clean_work *cw = container_of(work, struct clean_work, work);
 
         _mlx4_ib_mcg_port_cleanup(cw->ctx, cw->destroy_wq);
+        cw->ctx->flushing = 0;
         kfree(cw);
 }
 
@@ -1130,13 +1125,20 @@ void mlx4_ib_mcg_port_cleanup(struct mlx4_ib_demux_ctx *ctx, int destroy_wq)
 {
         struct clean_work *work;
 
+        if (ctx->flushing)
+                return;
+
+        ctx->flushing = 1;
+
         if (destroy_wq) {
                 _mlx4_ib_mcg_port_cleanup(ctx, destroy_wq);
+                ctx->flushing = 0;
                 return;
         }
 
         work = kmalloc(sizeof *work, GFP_KERNEL);
         if (!work) {
+                ctx->flushing = 0;
                 mcg_warn("failed allocating work for cleanup\n");
                 return;
         }
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index 9e1449f8c6a2..cf23c46185b2 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -3564,16 +3564,6 @@ static int srpt_get_tcm_cmd_state(struct se_cmd *se_cmd)
         return srpt_get_cmd_state(ioctx);
 }
 
-static u16 srpt_set_fabric_sense_len(struct se_cmd *cmd, u32 sense_length)
-{
-        return 0;
-}
-
-static u16 srpt_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
 /**
  * srpt_parse_i_port_id() - Parse an initiator port ID.
  * @name: ASCII representation of a 128-bit initiator port ID.
@@ -3953,8 +3943,6 @@ static struct target_core_fabric_ops srpt_template = {
         .queue_data_in                  = srpt_queue_response,
         .queue_status                   = srpt_queue_status,
         .queue_tm_rsp                   = srpt_queue_response,
-        .get_fabric_sense_len           = srpt_get_fabric_sense_len,
-        .set_fabric_sense_len           = srpt_set_fabric_sense_len,
         /*
          * Setup function pointers for generic logic in
          * target_core_fabric_configfs.c
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 118d0300f1fb..f0f8928b3c8a 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -23,11 +23,11 @@
 #include <linux/input/mt.h>
 #include <linux/major.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 #include "input-compat.h"
 
 struct evdev {
         int open;
-        int minor;
         struct input_handle handle;
         wait_queue_head_t wait;
         struct evdev_client __rcu *grab;
@@ -35,6 +35,7 @@ struct evdev {
         spinlock_t client_lock; /* protects client_list */
         struct mutex mutex;
         struct device dev;
+        struct cdev cdev;
         bool exist;
 };
 
@@ -51,9 +52,6 @@ struct evdev_client {
         struct input_event buffer[];
 };
 
-static struct evdev *evdev_table[EVDEV_MINORS];
-static DEFINE_MUTEX(evdev_table_mutex);
-
 static void __pass_event(struct evdev_client *client,
                          const struct input_event *event)
 {
@@ -294,7 +292,6 @@ static int evdev_release(struct inode *inode, struct file *file)
         kfree(client);
 
         evdev_close_device(evdev);
-        put_device(&evdev->dev);
 
         return 0;
 }
@@ -310,35 +307,16 @@ static unsigned int evdev_compute_buffer_size(struct input_dev *dev)
 
 static int evdev_open(struct inode *inode, struct file *file)
 {
-        struct evdev *evdev;
+        struct evdev *evdev = container_of(inode->i_cdev, struct evdev, cdev);
+        unsigned int bufsize = evdev_compute_buffer_size(evdev->handle.dev);
         struct evdev_client *client;
-        int i = iminor(inode) - EVDEV_MINOR_BASE;
-        unsigned int bufsize;
         int error;
 
-        if (i >= EVDEV_MINORS)
-                return -ENODEV;
-
-        error = mutex_lock_interruptible(&evdev_table_mutex);
-        if (error)
-                return error;
-        evdev = evdev_table[i];
-        if (evdev)
-                get_device(&evdev->dev);
-        mutex_unlock(&evdev_table_mutex);
-
-        if (!evdev)
-                return -ENODEV;
-
-        bufsize = evdev_compute_buffer_size(evdev->handle.dev);
-
         client = kzalloc(sizeof(struct evdev_client) +
                                 bufsize * sizeof(struct input_event),
                          GFP_KERNEL);
-        if (!client) {
-                error = -ENOMEM;
-                goto err_put_evdev;
-        }
+        if (!client)
+                return -ENOMEM;
 
         client->bufsize = bufsize;
         spin_lock_init(&client->buffer_lock);
@@ -357,8 +335,6 @@ static int evdev_open(struct inode *inode, struct file *file)
  err_free_client:
         evdev_detach_client(evdev, client);
         kfree(client);
- err_put_evdev:
-        put_device(&evdev->dev);
         return error;
 }
 
@@ -942,26 +918,6 @@ static const struct file_operations evdev_fops = {
         .llseek         = no_llseek,
 };
 
-static int evdev_install_chrdev(struct evdev *evdev)
-{
-        /*
-         * No need to do any locking here as calls to connect and
-         * disconnect are serialized by the input core
-         */
-        evdev_table[evdev->minor] = evdev;
-        return 0;
-}
-
-static void evdev_remove_chrdev(struct evdev *evdev)
-{
-        /*
-         * Lock evdev table to prevent race with evdev_open()
-         */
-        mutex_lock(&evdev_table_mutex);
-        evdev_table[evdev->minor] = NULL;
-        mutex_unlock(&evdev_table_mutex);
-}
-
 /*
  * Mark device non-existent. This disables writes, ioctls and
  * prevents new users from opening the device. Already posted
@@ -980,7 +936,8 @@ static void evdev_cleanup(struct evdev *evdev)
 
         evdev_mark_dead(evdev);
         evdev_hangup(evdev);
-        evdev_remove_chrdev(evdev);
+
+        cdev_del(&evdev->cdev);
 
         /* evdev is marked dead so no one else accesses evdev->open */
         if (evdev->open) {
@@ -991,43 +948,47 @@ static void evdev_cleanup(struct evdev *evdev)
 
 /*
  * Create new evdev device. Note that input core serializes calls
- * to connect and disconnect so we don't need to lock evdev_table here.
+ * to connect and disconnect.
  */
 static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
                          const struct input_device_id *id)
 {
         struct evdev *evdev;
         int minor;
+        int dev_no;
         int error;
 
-        for (minor = 0; minor < EVDEV_MINORS; minor++)
-                if (!evdev_table[minor])
-                        break;
-
-        if (minor == EVDEV_MINORS) {
-                pr_err("no more free evdev devices\n");
-                return -ENFILE;
+        minor = input_get_new_minor(EVDEV_MINOR_BASE, EVDEV_MINORS, true);
+        if (minor < 0) {
+                error = minor;
+                pr_err("failed to reserve new minor: %d\n", error);
+                return error;
         }
 
         evdev = kzalloc(sizeof(struct evdev), GFP_KERNEL);
-        if (!evdev)
-                return -ENOMEM;
+        if (!evdev) {
+                error = -ENOMEM;
+                goto err_free_minor;
+        }
 
         INIT_LIST_HEAD(&evdev->client_list);
         spin_lock_init(&evdev->client_lock);
         mutex_init(&evdev->mutex);
         init_waitqueue_head(&evdev->wait);
-
-        dev_set_name(&evdev->dev, "event%d", minor);
         evdev->exist = true;
-        evdev->minor = minor;
+
+        dev_no = minor;
+        /* Normalize device number if it falls into legacy range */
+        if (dev_no < EVDEV_MINOR_BASE + EVDEV_MINORS)
+                dev_no -= EVDEV_MINOR_BASE;
+        dev_set_name(&evdev->dev, "event%d", dev_no);
 
         evdev->handle.dev = input_get_device(dev);
         evdev->handle.name = dev_name(&evdev->dev);
         evdev->handle.handler = handler;
         evdev->handle.private = evdev;
 
-        evdev->dev.devt = MKDEV(INPUT_MAJOR, EVDEV_MINOR_BASE + minor);
+        evdev->dev.devt = MKDEV(INPUT_MAJOR, minor);
         evdev->dev.class = &input_class;
         evdev->dev.parent = &dev->dev;
         evdev->dev.release = evdev_free;
@@ -1037,7 +998,9 @@ static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
         if (error)
                 goto err_free_evdev;
 
-        error = evdev_install_chrdev(evdev);
+        cdev_init(&evdev->cdev, &evdev_fops);
+        evdev->cdev.kobj.parent = &evdev->dev.kobj;
+        error = cdev_add(&evdev->cdev, evdev->dev.devt, 1);
         if (error)
                 goto err_unregister_handle;
 
@@ -1053,6 +1016,8 @@ static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
         input_unregister_handle(&evdev->handle);
  err_free_evdev:
         put_device(&evdev->dev);
+ err_free_minor:
+        input_free_minor(minor);
         return error;
 }
 
@@ -1062,6 +1027,7 @@ static void evdev_disconnect(struct input_handle *handle)
 
         device_del(&evdev->dev);
         evdev_cleanup(evdev);
+        input_free_minor(MINOR(evdev->dev.devt));
         input_unregister_handle(handle);
         put_device(&evdev->dev);
 }
@@ -1078,7 +1044,7 @@ static struct input_handler evdev_handler = {
         .events         = evdev_events,
         .connect        = evdev_connect,
         .disconnect     = evdev_disconnect,
-        .fops           = &evdev_fops,
+        .legacy_minors  = true,
         .minor          = EVDEV_MINOR_BASE,
         .name           = "evdev",
         .id_table       = evdev_ids,
diff --git a/drivers/input/input.c b/drivers/input/input.c
index ace3f7c4226d..53a0ddee7872 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -14,6 +14,7 @@
 
 #include <linux/init.h>
 #include <linux/types.h>
+#include <linux/idr.h>
 #include <linux/input/mt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -32,7 +33,9 @@ MODULE_AUTHOR("Vojtech Pavlik <vojtech@suse.cz>");
 MODULE_DESCRIPTION("Input core");
 MODULE_LICENSE("GPL");
 
-#define INPUT_DEVICES   256
+#define INPUT_MAX_CHAR_DEVICES          1024
+#define INPUT_FIRST_DYNAMIC_DEV         256
+static DEFINE_IDA(input_ida);
 
 static LIST_HEAD(input_dev_list);
 static LIST_HEAD(input_handler_list);
@@ -45,8 +48,6 @@ static LIST_HEAD(input_handler_list);
  */
 static DEFINE_MUTEX(input_mutex);
 
-static struct input_handler *input_table[8];
-
 static const struct input_value input_value_sync = { EV_SYN, SYN_REPORT, 1 };
 
 static inline int is_event_supported(unsigned int code,
@@ -1218,7 +1219,7 @@ static int input_handlers_seq_show(struct seq_file *seq, void *v)
         seq_printf(seq, "N: Number=%u Name=%s", state->pos, handler->name);
         if (handler->filter)
                 seq_puts(seq, " (filter)");
-        if (handler->fops)
+        if (handler->legacy_minors)
                 seq_printf(seq, " Minor=%d", handler->minor);
         seq_putc(seq, '\n');
 
@@ -2016,22 +2017,14 @@ EXPORT_SYMBOL(input_unregister_device);
 int input_register_handler(struct input_handler *handler)
 {
         struct input_dev *dev;
-        int retval;
+        int error;
 
-        retval = mutex_lock_interruptible(&input_mutex);
-        if (retval)
-                return retval;
+        error = mutex_lock_interruptible(&input_mutex);
+        if (error)
+                return error;
 
         INIT_LIST_HEAD(&handler->h_list);
 
-        if (handler->fops != NULL) {
-                if (input_table[handler->minor >> 5]) {
-                        retval = -EBUSY;
-                        goto out;
-                }
-                input_table[handler->minor >> 5] = handler;
-        }
-
         list_add_tail(&handler->node, &input_handler_list);
 
         list_for_each_entry(dev, &input_dev_list, node)
@@ -2039,9 +2032,8 @@ int input_register_handler(struct input_handler *handler)
 
         input_wakeup_procfs_readers();
 
- out:
         mutex_unlock(&input_mutex);
-        return retval;
+        return 0;
 }
 EXPORT_SYMBOL(input_register_handler);
 
@@ -2064,9 +2056,6 @@ void input_unregister_handler(struct input_handler *handler)
 
         list_del_init(&handler->node);
 
-        if (handler->fops != NULL)
-                input_table[handler->minor >> 5] = NULL;
-
         input_wakeup_procfs_readers();
 
         mutex_unlock(&input_mutex);
@@ -2183,51 +2172,52 @@ void input_unregister_handle(struct input_handle *handle)
 }
 EXPORT_SYMBOL(input_unregister_handle);
 
-static int input_open_file(struct inode *inode, struct file *file)
+/**
+ * input_get_new_minor - allocates a new input minor number
+ * @legacy_base: beginning or the legacy range to be searched
+ * @legacy_num: size of legacy range
+ * @allow_dynamic: whether we can also take ID from the dynamic range
+ *
+ * This function allocates a new device minor for from input major namespace.
+ * Caller can request legacy minor by specifying @legacy_base and @legacy_num
+ * parameters and whether ID can be allocated from dynamic range if there are
+ * no free IDs in legacy range.
+ */
+int input_get_new_minor(int legacy_base, unsigned int legacy_num,
+                        bool allow_dynamic)
 {
-        struct input_handler *handler;
-        const struct file_operations *old_fops, *new_fops = NULL;
-        int err;
-
-        err = mutex_lock_interruptible(&input_mutex);
-        if (err)
-                return err;
-
-        /* No load-on-demand here? */
-        handler = input_table[iminor(inode) >> 5];
-        if (handler)
-                new_fops = fops_get(handler->fops);
-
-        mutex_unlock(&input_mutex);
-
         /*
-         * That's _really_ odd. Usually NULL ->open means "nothing special",
-         * not "no device". Oh, well...
+         * This function should be called from input handler's ->connect()
+         * methods, which are serialized with input_mutex, so no additional
+         * locking is needed here.
          */
-        if (!new_fops || !new_fops->open) {
-                fops_put(new_fops);
-                err = -ENODEV;
-                goto out;
+        if (legacy_base >= 0) {
+                int minor = ida_simple_get(&input_ida,
+                                           legacy_base,
+                                           legacy_base + legacy_num,
+                                           GFP_KERNEL);
+                if (minor >= 0 || !allow_dynamic)
+                        return minor;
         }
 
-        old_fops = file->f_op;
-        file->f_op = new_fops;
-
-        err = new_fops->open(inode, file);
-        if (err) {
-                fops_put(file->f_op);
-                file->f_op = fops_get(old_fops);
-        }
-        fops_put(old_fops);
-out:
-        return err;
+        return ida_simple_get(&input_ida,
+                              INPUT_FIRST_DYNAMIC_DEV, INPUT_MAX_CHAR_DEVICES,
+                              GFP_KERNEL);
 }
+EXPORT_SYMBOL(input_get_new_minor);
 
-static const struct file_operations input_fops = {
-        .owner = THIS_MODULE,
-        .open = input_open_file,
-        .llseek = noop_llseek,
-};
+/**
+ * input_free_minor - release previously allocated minor
+ * @minor: minor to be released
+ *
+ * This function releases previously allocated input minor so that it can be
+ * reused later.
+ */
+void input_free_minor(unsigned int minor)
+{
+        ida_simple_remove(&input_ida, minor);
+}
+EXPORT_SYMBOL(input_free_minor);
 
 static int __init input_init(void)
 {
@@ -2243,7 +2233,8 @@ static int __init input_init(void)
         if (err)
                 goto fail1;
 
-        err = register_chrdev(INPUT_MAJOR, "input", &input_fops);
+        err = register_chrdev_region(MKDEV(INPUT_MAJOR, 0),
+                                     INPUT_MAX_CHAR_DEVICES, "input");
         if (err) {
                 pr_err("unable to register char major %d", INPUT_MAJOR);
                 goto fail2;
@@ -2259,7 +2250,8 @@ static int __init input_init(void)
 static void __exit input_exit(void)
 {
         input_proc_exit();
-        unregister_chrdev(INPUT_MAJOR, "input");
+        unregister_chrdev_region(MKDEV(INPUT_MAJOR, 0),
+                                 INPUT_MAX_CHAR_DEVICES);
         class_unregister(&input_class);
 }
 
diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c
index 78f323ea1e4b..f362883c94e3 100644
--- a/drivers/input/joydev.c
+++ b/drivers/input/joydev.c
@@ -27,6 +27,7 @@
 #include <linux/poll.h>
 #include <linux/init.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 
 MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Joystick device interfaces");
@@ -39,13 +40,13 @@ MODULE_LICENSE("GPL");
 
 struct joydev {
         int open;
-        int minor;
         struct input_handle handle;
         wait_queue_head_t wait;
         struct list_head client_list;
         spinlock_t client_lock; /* protects client_list */
         struct mutex mutex;
         struct device dev;
+        struct cdev cdev;
         bool exist;
 
         struct js_corr corr[ABS_CNT];
@@ -70,9 +71,6 @@ struct joydev_client {
         struct list_head node;
 };
 
-static struct joydev *joydev_table[JOYDEV_MINORS];
-static DEFINE_MUTEX(joydev_table_mutex);
-
 static int joydev_correct(int value, struct js_corr *corr)
 {
         switch (corr->type) {
@@ -245,37 +243,20 @@ static int joydev_release(struct inode *inode, struct file *file)
         kfree(client);
 
         joydev_close_device(joydev);
-        put_device(&joydev->dev);
 
         return 0;
 }
 
 static int joydev_open(struct inode *inode, struct file *file)
 {
+        struct joydev *joydev =
+                        container_of(inode->i_cdev, struct joydev, cdev);
         struct joydev_client *client;
-        struct joydev *joydev;
-        int i = iminor(inode) - JOYDEV_MINOR_BASE;
         int error;
 
-        if (i >= JOYDEV_MINORS)
-                return -ENODEV;
-
-        error = mutex_lock_interruptible(&joydev_table_mutex);
-        if (error)
-                return error;
-        joydev = joydev_table[i];
-        if (joydev)
-                get_device(&joydev->dev);
-        mutex_unlock(&joydev_table_mutex);
-
-        if (!joydev)
-                return -ENODEV;
-
         client = kzalloc(sizeof(struct joydev_client), GFP_KERNEL);
-        if (!client) {
-                error = -ENOMEM;
-                goto err_put_joydev;
-        }
+        if (!client)
+                return -ENOMEM;
 
         spin_lock_init(&client->buffer_lock);
         client->joydev = joydev;
@@ -293,8 +274,6 @@ static int joydev_open(struct inode *inode, struct file *file)
  err_free_client:
         joydev_detach_client(joydev, client);
         kfree(client);
- err_put_joydev:
-        put_device(&joydev->dev);
         return error;
 }
 
@@ -742,19 +721,6 @@ static const struct file_operations joydev_fops = {
         .llseek         = no_llseek,
 };
 
-static int joydev_install_chrdev(struct joydev *joydev)
-{
-        joydev_table[joydev->minor] = joydev;
-        return 0;
-}
-
-static void joydev_remove_chrdev(struct joydev *joydev)
-{
-        mutex_lock(&joydev_table_mutex);
-        joydev_table[joydev->minor] = NULL;
-        mutex_unlock(&joydev_table_mutex);
-}
-
 /*
  * Mark device non-existent. This disables writes, ioctls and
  * prevents new users from opening the device. Already posted
@@ -773,7 +739,8 @@ static void joydev_cleanup(struct joydev *joydev)
 
         joydev_mark_dead(joydev);
         joydev_hangup(joydev);
-        joydev_remove_chrdev(joydev);
+
+        cdev_del(&joydev->cdev);
 
         /* joydev is marked dead so no one else accesses joydev->open */
         if (joydev->open)
@@ -798,30 +765,33 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
                           const struct input_device_id *id)
 {
         struct joydev *joydev;
-        int i, j, t, minor;
+        int i, j, t, minor, dev_no;
         int error;
 
-        for (minor = 0; minor < JOYDEV_MINORS; minor++)
-                if (!joydev_table[minor])
-                        break;
-
-        if (minor == JOYDEV_MINORS) {
-                pr_err("no more free joydev devices\n");
-                return -ENFILE;
+        minor = input_get_new_minor(JOYDEV_MINOR_BASE, JOYDEV_MINORS, true);
+        if (minor < 0) {
+                error = minor;
+                pr_err("failed to reserve new minor: %d\n", error);
+                return error;
         }
 
         joydev = kzalloc(sizeof(struct joydev), GFP_KERNEL);
-        if (!joydev)
-                return -ENOMEM;
+        if (!joydev) {
+                error = -ENOMEM;
+                goto err_free_minor;
+        }
 
         INIT_LIST_HEAD(&joydev->client_list);
         spin_lock_init(&joydev->client_lock);
         mutex_init(&joydev->mutex);
         init_waitqueue_head(&joydev->wait);
-
-        dev_set_name(&joydev->dev, "js%d", minor);
         joydev->exist = true;
-        joydev->minor = minor;
+
+        dev_no = minor;
+        /* Normalize device number if it falls into legacy range */
+        if (dev_no < JOYDEV_MINOR_BASE + JOYDEV_MINORS)
+                dev_no -= JOYDEV_MINOR_BASE;
+        dev_set_name(&joydev->dev, "js%d", dev_no);
 
         joydev->handle.dev = input_get_device(dev);
         joydev->handle.name = dev_name(&joydev->dev);
@@ -875,7 +845,7 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
                 }
         }
 
-        joydev->dev.devt = MKDEV(INPUT_MAJOR, JOYDEV_MINOR_BASE + minor);
+        joydev->dev.devt = MKDEV(INPUT_MAJOR, minor);
         joydev->dev.class = &input_class;
         joydev->dev.parent = &dev->dev;
         joydev->dev.release = joydev_free;
@@ -885,7 +855,9 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
         if (error)
                 goto err_free_joydev;
 
-        error = joydev_install_chrdev(joydev);
+        cdev_init(&joydev->cdev, &joydev_fops);
+        joydev->cdev.kobj.parent = &joydev->dev.kobj;
+        error = cdev_add(&joydev->cdev, joydev->dev.devt, 1);
         if (error)
                 goto err_unregister_handle;
 
@@ -901,6 +873,8 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
         input_unregister_handle(&joydev->handle);
  err_free_joydev:
         put_device(&joydev->dev);
+ err_free_minor:
+        input_free_minor(minor);
         return error;
 }
 
@@ -910,6 +884,7 @@ static void joydev_disconnect(struct input_handle *handle)
 
         device_del(&joydev->dev);
         joydev_cleanup(joydev);
+        input_free_minor(MINOR(joydev->dev.devt));
         input_unregister_handle(handle);
         put_device(&joydev->dev);
 }
@@ -961,7 +936,7 @@ static struct input_handler joydev_handler = {
         .match          = joydev_match,
         .connect        = joydev_connect,
         .disconnect     = joydev_disconnect,
-        .fops           = &joydev_fops,
+        .legacy_minors  = true,
         .minor          = JOYDEV_MINOR_BASE,
         .name           = "joydev",
         .id_table       = joydev_ids,
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index b4b65af8612a..de0874054e9f 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -335,6 +335,7 @@ config KEYBOARD_LOCOMO
 config KEYBOARD_LPC32XX
         tristate "LPC32XX matrix key scanner support"
         depends on ARCH_LPC32XX && OF
+        select INPUT_MATRIXKMAP
         help
           Say Y here if you want to use NXP LPC32XX SoC key scanner interface,
           connected to a key matrix.
diff --git a/drivers/input/keyboard/pxa27x_keypad.c b/drivers/input/keyboard/pxa27x_keypad.c
index 803ff6fe021e..cad9d5dd5973 100644
--- a/drivers/input/keyboard/pxa27x_keypad.c
+++ b/drivers/input/keyboard/pxa27x_keypad.c
@@ -368,6 +368,9 @@ static void pxa27x_keypad_config(struct pxa27x_keypad *keypad)
         unsigned int mask = 0, direct_key_num = 0;
         unsigned long kpc = 0;
 
+        /* clear pending interrupt bit */
+        keypad_readl(KPC);
+
         /* enable matrix keys with automatic scan */
         if (pdata->matrix_key_rows && pdata->matrix_key_cols) {
                 kpc |= KPC_ASACT | KPC_MIE | KPC_ME | KPC_MS_ALL;
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index 277e26dc910e..9d7a111486f7 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -431,6 +431,12 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
                 goto err_unmap_base;
         }
 
+        error = clk_prepare(keypad->clk);
+        if (error) {
+                dev_err(&pdev->dev, "keypad clock prepare failed\n");
+                goto err_put_clk;
+        }
+
         keypad->input_dev = input_dev;
         keypad->pdev = pdev;
         keypad->row_shift = row_shift;
@@ -461,7 +467,7 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
                                            keypad->keycodes, input_dev);
         if (error) {
                 dev_err(&pdev->dev, "failed to build keymap\n");
-                goto err_put_clk;
+                goto err_unprepare_clk;
         }
 
         input_set_capability(input_dev, EV_MSC, MSC_SCAN);
@@ -503,6 +509,8 @@ err_free_irq:
         pm_runtime_disable(&pdev->dev);
         device_init_wakeup(&pdev->dev, 0);
         platform_set_drvdata(pdev, NULL);
+err_unprepare_clk:
+        clk_unprepare(keypad->clk);
 err_put_clk:
         clk_put(keypad->clk);
         samsung_keypad_dt_gpio_free(keypad);
@@ -531,6 +539,7 @@ static int __devexit samsung_keypad_remove(struct platform_device *pdev)
          */
         free_irq(keypad->irq, keypad);
 
+        clk_unprepare(keypad->clk);
         clk_put(keypad->clk);
         samsung_keypad_dt_gpio_free(keypad);
 
diff --git a/drivers/input/misc/xen-kbdfront.c b/drivers/input/misc/xen-kbdfront.c
index 02ca8680ea5b..6f7d99013031 100644
--- a/drivers/input/misc/xen-kbdfront.c
+++ b/drivers/input/misc/xen-kbdfront.c
@@ -311,7 +311,6 @@ static void xenkbd_backend_changed(struct xenbus_device *dev,
         case XenbusStateReconfiguring:
         case XenbusStateReconfigured:
         case XenbusStateUnknown:
-        case XenbusStateClosed:
                 break;
 
         case XenbusStateInitWait:
@@ -350,6 +349,10 @@ InitWait:
 
                 break;
 
+        case XenbusStateClosed:
+                if (dev->state == XenbusStateClosed)
+                        break;
+                /* Missed the backend's CLOSING state -- fallthrough */
         case XenbusStateClosing:
                 xenbus_frontend_closed(dev);
                 break;
diff --git a/drivers/input/mouse/bcm5974.c b/drivers/input/mouse/bcm5974.c
index 3a78f235fa3e..2baff1b79a55 100644
--- a/drivers/input/mouse/bcm5974.c
+++ b/drivers/input/mouse/bcm5974.c
@@ -84,6 +84,10 @@
 #define USB_DEVICE_ID_APPLE_WELLSPRING7_ANSI    0x0262
 #define USB_DEVICE_ID_APPLE_WELLSPRING7_ISO     0x0263
 #define USB_DEVICE_ID_APPLE_WELLSPRING7_JIS     0x0264
+/* MacbookPro10,2 (unibody, October 2012) */
+#define USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI   0x0259
+#define USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO    0x025a
+#define USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS    0x025b
 
 #define BCM5974_DEVICE(prod) {                                  \
         .match_flags = (USB_DEVICE_ID_MATCH_DEVICE |            \
@@ -137,6 +141,10 @@ static const struct usb_device_id bcm5974_table[] = {
         BCM5974_DEVICE(USB_DEVICE_ID_APPLE_WELLSPRING7_ANSI),
         BCM5974_DEVICE(USB_DEVICE_ID_APPLE_WELLSPRING7_ISO),
         BCM5974_DEVICE(USB_DEVICE_ID_APPLE_WELLSPRING7_JIS),
+        /* MacbookPro10,2 */
+        BCM5974_DEVICE(USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI),
+        BCM5974_DEVICE(USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO),
+        BCM5974_DEVICE(USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS),
         /* Terminating entry */
         {}
 };
@@ -379,6 +387,19 @@ static const struct bcm5974_config bcm5974_config_table[] = {
                 { SN_COORD, -150, 6730 },
                 { SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
         },
+        {
+                USB_DEVICE_ID_APPLE_WELLSPRING7A_ANSI,
+                USB_DEVICE_ID_APPLE_WELLSPRING7A_ISO,
+                USB_DEVICE_ID_APPLE_WELLSPRING7A_JIS,
+                HAS_INTEGRATED_BUTTON,
+                0x84, sizeof(struct bt_data),
+                0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
+                { SN_PRESSURE, 0, 300 },
+                { SN_WIDTH, 0, 2048 },
+                { SN_COORD, -4750, 5280 },
+                { SN_COORD, -150, 6730 },
+                { SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
+        },
         {}
 };
 
diff --git a/drivers/input/mousedev.c b/drivers/input/mousedev.c
index 964e43d81e29..8f02e3d0e712 100644
--- a/drivers/input/mousedev.c
+++ b/drivers/input/mousedev.c
@@ -24,10 +24,8 @@
 #include <linux/random.h>
 #include <linux/major.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 #include <linux/kernel.h>
-#ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
-#include <linux/miscdevice.h>
-#endif
 
 MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Mouse (ExplorerPS/2) device interfaces");
@@ -61,17 +59,18 @@ struct mousedev_hw_data {
 
 struct mousedev {
         int open;
-        int minor;
         struct input_handle handle;
         wait_queue_head_t wait;
         struct list_head client_list;
         spinlock_t client_lock; /* protects client_list */
         struct mutex mutex;
         struct device dev;
+        struct cdev cdev;
         bool exist;
+        bool is_mixdev;
 
         struct list_head mixdev_node;
-        int mixdev_open;
+        bool opened_by_mixdev;
 
         struct mousedev_hw_data packet;
         unsigned int pkt_count;
@@ -114,10 +113,6 @@ struct mousedev_client {
 static unsigned char mousedev_imps_seq[] = { 0xf3, 200, 0xf3, 100, 0xf3, 80 };
 static unsigned char mousedev_imex_seq[] = { 0xf3, 200, 0xf3, 200, 0xf3, 80 };
 
-static struct input_handler mousedev_handler;
-
-static struct mousedev *mousedev_table[MOUSEDEV_MINORS];
-static DEFINE_MUTEX(mousedev_table_mutex);
 static struct mousedev *mousedev_mix;
 static LIST_HEAD(mousedev_mix_list);
 
@@ -433,7 +428,7 @@ static int mousedev_open_device(struct mousedev *mousedev)
         if (retval)
                 return retval;
 
-        if (mousedev->minor == MOUSEDEV_MIX)
+        if (mousedev->is_mixdev)
                 mixdev_open_devices();
         else if (!mousedev->exist)
                 retval = -ENODEV;
@@ -451,7 +446,7 @@ static void mousedev_close_device(struct mousedev *mousedev)
 {
         mutex_lock(&mousedev->mutex);
 
-        if (mousedev->minor == MOUSEDEV_MIX)
+        if (mousedev->is_mixdev)
                 mixdev_close_devices();
         else if (mousedev->exist && !--mousedev->open)
                 input_close_device(&mousedev->handle);
@@ -472,11 +467,11 @@ static void mixdev_open_devices(void)
                 return;
 
         list_for_each_entry(mousedev, &mousedev_mix_list, mixdev_node) {
-                if (!mousedev->mixdev_open) {
+                if (!mousedev->opened_by_mixdev) {
                         if (mousedev_open_device(mousedev))
                                 continue;
 
-                        mousedev->mixdev_open = 1;
+                        mousedev->opened_by_mixdev = true;
                 }
         }
 }
@@ -494,8 +489,8 @@ static void mixdev_close_devices(void)
                 return;
 
         list_for_each_entry(mousedev, &mousedev_mix_list, mixdev_node) {
-                if (mousedev->mixdev_open) {
-                        mousedev->mixdev_open = 0;
+                if (mousedev->opened_by_mixdev) {
+                        mousedev->opened_by_mixdev = false;
                         mousedev_close_device(mousedev);
                 }
         }
@@ -528,7 +523,6 @@ static int mousedev_release(struct inode *inode, struct file *file)
         kfree(client);
 
         mousedev_close_device(mousedev);
-        put_device(&mousedev->dev);
 
         return 0;
 }
@@ -538,35 +532,17 @@ static int mousedev_open(struct inode *inode, struct file *file)
         struct mousedev_client *client;
         struct mousedev *mousedev;
         int error;
-        int i;
 
 #ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
         if (imajor(inode) == MISC_MAJOR)
-                i = MOUSEDEV_MIX;
+                mousedev = mousedev_mix;
         else
 #endif
-                i = iminor(inode) - MOUSEDEV_MINOR_BASE;
-
-        if (i >= MOUSEDEV_MINORS)
-                return -ENODEV;
-
-        error = mutex_lock_interruptible(&mousedev_table_mutex);
-        if (error)
-                return error;
-
-        mousedev = mousedev_table[i];
-        if (mousedev)
-                get_device(&mousedev->dev);
-        mutex_unlock(&mousedev_table_mutex);
-
-        if (!mousedev)
-                return -ENODEV;
+                mousedev = container_of(inode->i_cdev, struct mousedev, cdev);
 
         client = kzalloc(sizeof(struct mousedev_client), GFP_KERNEL);
-        if (!client) {
-                error = -ENOMEM;
-                goto err_put_mousedev;
-        }
+        if (!client)
+                return -ENOMEM;
 
         spin_lock_init(&client->packet_lock);
         client->pos_x = xres / 2;
@@ -579,13 +555,13 @@ static int mousedev_open(struct inode *inode, struct file *file)
                 goto err_free_client;
 
         file->private_data = client;
+        nonseekable_open(inode, file);
+
         return 0;
 
  err_free_client:
         mousedev_detach_client(mousedev, client);
         kfree(client);
- err_put_mousedev:
-        put_device(&mousedev->dev);
         return error;
 }
 
@@ -785,29 +761,16 @@ static unsigned int mousedev_poll(struct file *file, poll_table *wait)
 }
 
 static const struct file_operations mousedev_fops = {
-        .owner =        THIS_MODULE,
-        .read =         mousedev_read,
-        .write =        mousedev_write,
-        .poll =         mousedev_poll,
-        .open =         mousedev_open,
-        .release =      mousedev_release,
-        .fasync =       mousedev_fasync,
-        .llseek = noop_llseek,
+        .owner          = THIS_MODULE,
+        .read           = mousedev_read,
+        .write          = mousedev_write,
+        .poll           = mousedev_poll,
+        .open           = mousedev_open,
+        .release        = mousedev_release,
+        .fasync         = mousedev_fasync,
+        .llseek         = noop_llseek,
 };
 
-static int mousedev_install_chrdev(struct mousedev *mousedev)
-{
-        mousedev_table[mousedev->minor] = mousedev;
-        return 0;
-}
-
-static void mousedev_remove_chrdev(struct mousedev *mousedev)
-{
-        mutex_lock(&mousedev_table_mutex);
-        mousedev_table[mousedev->minor] = NULL;
-        mutex_unlock(&mousedev_table_mutex);
-}
-
 /*
  * Mark device non-existent. This disables writes, ioctls and
  * prevents new users from opening the device. Already posted
@@ -842,24 +805,50 @@ static void mousedev_cleanup(struct mousedev *mousedev)
 
         mousedev_mark_dead(mousedev);
         mousedev_hangup(mousedev);
-        mousedev_remove_chrdev(mousedev);
+
+        cdev_del(&mousedev->cdev);
 
         /* mousedev is marked dead so no one else accesses mousedev->open */
         if (mousedev->open)
                 input_close_device(handle);
 }
 
+static int mousedev_reserve_minor(bool mixdev)
+{
+        int minor;
+
+        if (mixdev) {
+                minor = input_get_new_minor(MOUSEDEV_MIX, 1, false);
+                if (minor < 0)
+                        pr_err("failed to reserve mixdev minor: %d\n", minor);
+        } else {
+                minor = input_get_new_minor(MOUSEDEV_MINOR_BASE,
+                                            MOUSEDEV_MINORS, true);
+                if (minor < 0)
+                        pr_err("failed to reserve new minor: %d\n", minor);
+        }
+
+        return minor;
+}
+
 static struct mousedev *mousedev_create(struct input_dev *dev,
                                         struct input_handler *handler,
-                                        int minor)
+                                        bool mixdev)
 {
         struct mousedev *mousedev;
+        int minor;
         int error;
 
+        minor = mousedev_reserve_minor(mixdev);
+        if (minor < 0) {
+                error = minor;
+                goto err_out;
+        }
+
         mousedev = kzalloc(sizeof(struct mousedev), GFP_KERNEL);
         if (!mousedev) {
                 error = -ENOMEM;
-                goto err_out;
+                goto err_free_minor;
         }
 
         INIT_LIST_HEAD(&mousedev->client_list);
@@ -867,16 +856,21 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
         spin_lock_init(&mousedev->client_lock);
         mutex_init(&mousedev->mutex);
         lockdep_set_subclass(&mousedev->mutex,
-                             minor == MOUSEDEV_MIX ? SINGLE_DEPTH_NESTING : 0);
+                             mixdev ? SINGLE_DEPTH_NESTING : 0);
         init_waitqueue_head(&mousedev->wait);
 
-        if (minor == MOUSEDEV_MIX)
+        if (mixdev) {
                 dev_set_name(&mousedev->dev, "mice");
-        else
-                dev_set_name(&mousedev->dev, "mouse%d", minor);
+        } else {
+                int dev_no = minor;
+                /* Normalize device number if it falls into legacy range */
+                if (dev_no < MOUSEDEV_MINOR_BASE + MOUSEDEV_MINORS)
+                        dev_no -= MOUSEDEV_MINOR_BASE;
+                dev_set_name(&mousedev->dev, "mouse%d", dev_no);
+        }
 
-        mousedev->minor = minor;
         mousedev->exist = true;
+        mousedev->is_mixdev = mixdev;
         mousedev->handle.dev = input_get_device(dev);
         mousedev->handle.name = dev_name(&mousedev->dev);
         mousedev->handle.handler = handler;
@@ -885,17 +879,19 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
         mousedev->dev.class = &input_class;
         if (dev)
                 mousedev->dev.parent = &dev->dev;
-        mousedev->dev.devt = MKDEV(INPUT_MAJOR, MOUSEDEV_MINOR_BASE + minor);
+        mousedev->dev.devt = MKDEV(INPUT_MAJOR, minor);
         mousedev->dev.release = mousedev_free;
         device_initialize(&mousedev->dev);
 
-        if (minor != MOUSEDEV_MIX) {
+        if (!mixdev) {
                 error = input_register_handle(&mousedev->handle);
                 if (error)
                         goto err_free_mousedev;
         }
 
-        error = mousedev_install_chrdev(mousedev);
+        cdev_init(&mousedev->cdev, &mousedev_fops);
+        mousedev->cdev.kobj.parent = &mousedev->dev.kobj;
+        error = cdev_add(&mousedev->cdev, mousedev->dev.devt, 1);
         if (error)
                 goto err_unregister_handle;
 
@@ -908,10 +904,12 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
  err_cleanup_mousedev:
         mousedev_cleanup(mousedev);
  err_unregister_handle:
-        if (minor != MOUSEDEV_MIX)
+        if (!mixdev)
                 input_unregister_handle(&mousedev->handle);
  err_free_mousedev:
         put_device(&mousedev->dev);
+ err_free_minor:
+        input_free_minor(minor);
  err_out:
         return ERR_PTR(error);
 }
@@ -920,7 +918,8 @@ static void mousedev_destroy(struct mousedev *mousedev)
 {
         device_del(&mousedev->dev);
         mousedev_cleanup(mousedev);
-        if (mousedev->minor != MOUSEDEV_MIX)
+        input_free_minor(MINOR(mousedev->dev.devt));
+        if (!mousedev->is_mixdev)
                 input_unregister_handle(&mousedev->handle);
         put_device(&mousedev->dev);
 }
@@ -938,7 +937,7 @@ static int mixdev_add_device(struct mousedev *mousedev)
                 if (retval)
                         goto out;
 
-                mousedev->mixdev_open = 1;
+                mousedev->opened_by_mixdev = true;
         }
 
         get_device(&mousedev->dev);
@@ -953,8 +952,8 @@ static void mixdev_remove_device(struct mousedev *mousedev)
 {
         mutex_lock(&mousedev_mix->mutex);
 
-        if (mousedev->mixdev_open) {
-                mousedev->mixdev_open = 0;
+        if (mousedev->opened_by_mixdev) {
+                mousedev->opened_by_mixdev = false;
                 mousedev_close_device(mousedev);
         }
 
@@ -969,19 +968,9 @@ static int mousedev_connect(struct input_handler *handler,
                             const struct input_device_id *id)
 {
         struct mousedev *mousedev;
-        int minor;
         int error;
 
-        for (minor = 0; minor < MOUSEDEV_MINORS; minor++)
-                if (!mousedev_table[minor])
-                        break;
-
-        if (minor == MOUSEDEV_MINORS) {
-                pr_err("no more free mousedev devices\n");
-                return -ENFILE;
-        }
-
-        mousedev = mousedev_create(dev, handler, minor);
+        mousedev = mousedev_create(dev, handler, false);
         if (IS_ERR(mousedev))
                 return PTR_ERR(mousedev);
 
@@ -1054,27 +1043,53 @@ static const struct input_device_id mousedev_ids[] = {
 MODULE_DEVICE_TABLE(input, mousedev_ids);
 
 static struct input_handler mousedev_handler = {
-        .event =        mousedev_event,
-        .connect =      mousedev_connect,
-        .disconnect =   mousedev_disconnect,
-        .fops =         &mousedev_fops,
-        .minor =        MOUSEDEV_MINOR_BASE,
-        .name =         "mousedev",
-        .id_table =     mousedev_ids,
+        .event          = mousedev_event,
+        .connect        = mousedev_connect,
+        .disconnect     = mousedev_disconnect,
+        .legacy_minors  = true,
+        .minor          = MOUSEDEV_MINOR_BASE,
+        .name           = "mousedev",
+        .id_table       = mousedev_ids,
 };
 
 #ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
+#include <linux/miscdevice.h>
+
 static struct miscdevice psaux_mouse = {
-        PSMOUSE_MINOR, "psaux", &mousedev_fops
+        .minor  = PSMOUSE_MINOR,
+        .name   = "psaux",
+        .fops   = &mousedev_fops,
 };
-static int psaux_registered;
+
+static bool psaux_registered;
+
+static void __init mousedev_psaux_register(void)
+{
+        int error;
+
+        error = misc_register(&psaux_mouse);
+        if (error)
+                pr_warn("could not register psaux device, error: %d\n",
+                           error);
+        else
+                psaux_registered = true;
+}
+
+static void __exit mousedev_psaux_unregister(void)
+{
+        if (psaux_registered)
+                misc_deregister(&psaux_mouse);
+}
+#else
+static inline void mousedev_psaux_register(void) { }
+static inline void mousedev_psaux_unregister(void) { }
 #endif
 
 static int __init mousedev_init(void)
 {
         int error;
 
-        mousedev_mix = mousedev_create(NULL, &mousedev_handler, MOUSEDEV_MIX);
+        mousedev_mix = mousedev_create(NULL, &mousedev_handler, true);
         if (IS_ERR(mousedev_mix))
                 return PTR_ERR(mousedev_mix);
 
@@ -1084,14 +1099,7 @@ static int __init mousedev_init(void)
                 return error;
         }
 
-#ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
-        error = misc_register(&psaux_mouse);
-        if (error)
-                pr_warn("could not register psaux device, error: %d\n",
-                           error);
-        else
-                psaux_registered = 1;
-#endif
+        mousedev_psaux_register();
 
         pr_info("PS/2 mouse device common for all mice\n");
 
@@ -1100,10 +1108,7 @@ static int __init mousedev_init(void)
 
 static void __exit mousedev_exit(void)
 {
-#ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
-        if (psaux_registered)
-                misc_deregister(&psaux_mouse);
-#endif
+        mousedev_psaux_unregister();
         input_unregister_handler(&mousedev_handler);
         mousedev_destroy(mousedev_mix);
 }
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 0d3219f29744..858ad446de91 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -172,6 +172,76 @@ static void wacom_close(struct input_dev *dev)
 }
 
 /*
+ * Calculate the resolution of the X or Y axis, given appropriate HID data.
+ * This function is little more than hidinput_calc_abs_res stripped down.
+ */
+static int wacom_calc_hid_res(int logical_extents, int physical_extents,
+                              unsigned char unit, unsigned char exponent)
+{
+        int prev, unit_exponent;
+
+        /* Check if the extents are sane */
+        if (logical_extents <= 0 || physical_extents <= 0)
+                return 0;
+
+        /* Get signed value of nybble-sized twos-compliment exponent */
+        unit_exponent = exponent;
+        if (unit_exponent > 7)
+                unit_exponent -= 16;
+
+        /* Convert physical_extents to millimeters */
+        if (unit == 0x11) {             /* If centimeters */
+                unit_exponent += 1;
+        } else if (unit == 0x13) {      /* If inches */
+                prev = physical_extents;
+                physical_extents *= 254;
+                if (physical_extents < prev)
+                        return 0;
+                unit_exponent -= 1;
+        } else {
+                return 0;
+        }
+
+        /* Apply negative unit exponent */
+        for (; unit_exponent < 0; unit_exponent++) {
+                prev = logical_extents;
+                logical_extents *= 10;
+                if (logical_extents < prev)
+                        return 0;
+        }
+        /* Apply positive unit exponent */
+        for (; unit_exponent > 0; unit_exponent--) {
+                prev = physical_extents;
+                physical_extents *= 10;
+                if (physical_extents < prev)
+                        return 0;
+        }
+
+        /* Calculate resolution */
+        return logical_extents / physical_extents;
+}
+
+/*
+ * The physical dimension specified by the HID descriptor is likely not in
+ * the "100th of a mm" units expected by wacom_calculate_touch_res. This
+ * function adjusts the value of [xy]_phy based on the unit and exponent
+ * provided by the HID descriptor. If an error occurs durring conversion
+ * (e.g. from the unit being left unspecified) [xy]_phy is not modified.
+ */
+static void wacom_fix_phy_from_hid(struct wacom_features *features)
+{
+        int xres = wacom_calc_hid_res(features->x_max, features->x_phy,
+                                        features->unit, features->unitExpo);
+        int yres = wacom_calc_hid_res(features->y_max, features->y_phy,
+                                        features->unit, features->unitExpo);
+
+        if (xres > 0 && yres > 0) {
+                features->x_phy = (100 * features->x_max) / xres;
+                features->y_phy = (100 * features->y_max) / yres;
+        }
+}
+
+/*
  * Static values for max X/Y and resolution of Pen interface is stored in
  * features. This mean physical size of active area can be computed.
  * This is useful to do when Pen and Touch have same active area of tablet.
@@ -321,7 +391,7 @@ static int wacom_parse_hid(struct usb_interface *intf,
                                                         features->pktlen = WACOM_PKGLEN_TPC2FG;
                                                 }
 
-                                                if (features->type == MTSCREEN)
+                                                if (features->type == MTSCREEN || features->type == WACOM_24HDT)
                                                         features->pktlen = WACOM_PKGLEN_MTOUCH;
 
                                                 if (features->type == BAMBOO_PT) {
@@ -332,6 +402,14 @@ static int wacom_parse_hid(struct usb_interface *intf,
                                                         features->x_max =
                                                                 get_unaligned_le16(&report[i + 8]);
                                                         i += 15;
+                                                } else if (features->type == WACOM_24HDT) {
+                                                        features->x_max =
+                                                                get_unaligned_le16(&report[i + 3]);
+                                                        features->x_phy =
+                                                                get_unaligned_le16(&report[i + 8]);
+                                                        features->unit = report[i - 1];
+                                                        features->unitExpo = report[i - 3];
+                                                        i += 12;
                                                 } else {
                                                         features->x_max =
                                                                 get_unaligned_le16(&report[i + 3]);
@@ -364,6 +442,12 @@ static int wacom_parse_hid(struct usb_interface *intf,
                                                         features->y_phy =
                                                                 get_unaligned_le16(&report[i + 6]);
                                                         i += 7;
+                                                } else if (type == WACOM_24HDT) {
+                                                        features->y_max =
+                                                                get_unaligned_le16(&report[i + 3]);
+                                                        features->y_phy =
+                                                                get_unaligned_le16(&report[i - 2]);
+                                                        i += 7;
                                                 } else if (type == BAMBOO_PT) {
                                                         features->y_phy =
                                                                 get_unaligned_le16(&report[i + 3]);
@@ -432,56 +516,55 @@ static int wacom_parse_hid(struct usb_interface *intf,
         return result;
 }
 
-static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_features *features)
+static int wacom_set_device_mode(struct usb_interface *intf, int report_id, int length, int mode)
 {
         unsigned char *rep_data;
-        int limit = 0, report_id = 2;
-        int error = -ENOMEM;
+        int error = -ENOMEM, limit = 0;
 
-        rep_data = kmalloc(4, GFP_KERNEL);
+        rep_data = kzalloc(length, GFP_KERNEL);
         if (!rep_data)
                 return error;
 
-        /* ask to report Wacom data */
+        rep_data[0] = report_id;
+        rep_data[1] = mode;
+
+        do {
+                error = wacom_set_report(intf, WAC_HID_FEATURE_REPORT,
+                                         report_id, rep_data, length, 1);
+                if (error >= 0)
+                        error = wacom_get_report(intf, WAC_HID_FEATURE_REPORT,
+                                                 report_id, rep_data, length, 1);
+        } while ((error < 0 || rep_data[1] != mode) && limit++ < WAC_MSG_RETRIES);
+
+        kfree(rep_data);
+
+        return error < 0 ? error : 0;
+}
+
+/*
+ * Switch the tablet into its most-capable mode. Wacom tablets are
+ * typically configured to power-up in a mode which sends mouse-like
+ * reports to the OS. To get absolute position, pressure data, etc.
+ * from the tablet, it is necessary to switch the tablet out of this
+ * mode and into one which sends the full range of tablet data.
+ */
+static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_features *features)
+{
         if (features->device_type == BTN_TOOL_FINGER) {
-                /* if it is an MT Tablet PC touch */
                 if (features->type > TABLETPC) {
-                        do {
-                                rep_data[0] = 3;
-                                rep_data[1] = 4;
-                                rep_data[2] = 0;
-                                rep_data[3] = 0;
-                                report_id = 3;
-                                error = wacom_set_report(intf,
-                                                         WAC_HID_FEATURE_REPORT,
-                                                         report_id,
-                                                         rep_data, 4, 1);
-                                if (error >= 0)
-                                        error = wacom_get_report(intf,
-                                                        WAC_HID_FEATURE_REPORT,
-                                                        report_id,
-                                                        rep_data, 4, 1);
-                        } while ((error < 0 || rep_data[1] != 4) &&
-                                 limit++ < WAC_MSG_RETRIES);
+                        /* MT Tablet PC touch */
+                        return wacom_set_device_mode(intf, 3, 4, 4);
+                }
+                else if (features->type == WACOM_24HDT) {
+                        return wacom_set_device_mode(intf, 18, 3, 2);
+                }
+        } else if (features->device_type == BTN_TOOL_PEN) {
+                if (features->type <= BAMBOO_PT && features->type != WIRELESS) {
+                        return wacom_set_device_mode(intf, 2, 2, 2);
                 }
-        } else if (features->type <= BAMBOO_PT &&
-                   features->type != WIRELESS &&
-                   features->device_type == BTN_TOOL_PEN) {
-                do {
-                        rep_data[0] = 2;
-                        rep_data[1] = 2;
-                        error = wacom_set_report(intf, WAC_HID_FEATURE_REPORT,
-                                                 report_id, rep_data, 2, 1);
-                        if (error >= 0)
-                                error = wacom_get_report(intf,
-                                                WAC_HID_FEATURE_REPORT,
-                                                report_id, rep_data, 2, 1);
-                } while ((error < 0 || rep_data[1] != 2) && limit++ < WAC_MSG_RETRIES);
         }
 
-        kfree(rep_data);
-
-        return error < 0 ? error : 0;
+        return 0;
 }
 
 static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
@@ -531,6 +614,7 @@ static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
         error = wacom_parse_hid(intf, hid_desc, features);
         if (error)
                 goto out;
+        wacom_fix_phy_from_hid(features);
 
  out:
         return error;
@@ -546,6 +630,30 @@ struct wacom_usbdev_data {
 static LIST_HEAD(wacom_udev_list);
 static DEFINE_MUTEX(wacom_udev_list_lock);
 
+static struct usb_device *wacom_get_sibling(struct usb_device *dev, int vendor, int product)
+{
+        int port1;
+        struct usb_device *sibling;
+
+        if (vendor == 0 && product == 0)
+                return dev;
+
+        if (dev->parent == NULL)
+                return NULL;
+
+        usb_hub_for_each_child(dev->parent, port1, sibling) {
+                struct usb_device_descriptor *d;
+                if (sibling == NULL)
+                        continue;
+
+                d = &sibling->descriptor;
+                if (d->idVendor == vendor && d->idProduct == product)
+                        return sibling;
+        }
+
+        return NULL;
+}
+
 static struct wacom_usbdev_data *wacom_get_usbdev_data(struct usb_device *dev)
 {
         struct wacom_usbdev_data *data;
@@ -1190,13 +1298,19 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
         strlcpy(wacom_wac->name, features->name, sizeof(wacom_wac->name));
 
         if (features->quirks & WACOM_QUIRK_MULTI_INPUT) {
+                struct usb_device *other_dev;
+
                 /* Append the device type to the name */
                 strlcat(wacom_wac->name,
                         features->device_type == BTN_TOOL_PEN ?
                                 " Pen" : " Finger",
                         sizeof(wacom_wac->name));
 
-                error = wacom_add_shared_data(wacom_wac, dev);
+
+                other_dev = wacom_get_sibling(dev, features->oVid, features->oPid);
+                if (other_dev == NULL || wacom_get_usbdev_data(other_dev) == NULL)
+                        other_dev = dev;
+                error = wacom_add_shared_data(wacom_wac, other_dev);
                 if (error)
                         goto fail3;
         }
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 08b462b6c0d8..0a67031ffc13 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -25,6 +25,11 @@
 #define WACOM_INTUOS_RES        100
 #define WACOM_INTUOS3_RES       200
 
+/* Scale factor relating reported contact size to logical contact area.
+ * 2^14/pi is a good approximation on Intuos5 and 3rd-gen Bamboo
+ */
+#define WACOM_CONTACT_AREA_SCALE 2607
+
 static int wacom_penpartner_irq(struct wacom_wac *wacom)
 {
         unsigned char *data = wacom->data;
@@ -326,7 +331,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
         /* Enter report */
         if ((data[1] & 0xfc) == 0xc0) {
-                if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+                if (features->quirks == WACOM_QUIRK_MULTI_INPUT)
                         wacom->shared->stylus_in_proximity = true;
 
                 /* serial number of the tool */
@@ -414,7 +419,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
         /* Exit report */
         if ((data[1] & 0xfe) == 0x80) {
-                if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+                if (features->quirks == WACOM_QUIRK_MULTI_INPUT)
                         wacom->shared->stylus_in_proximity = false;
 
                 /*
@@ -801,6 +806,70 @@ static int find_slot_from_contactid(struct wacom_wac *wacom, int contactid)
         return -1;
 }
 
+static int int_dist(int x1, int y1, int x2, int y2)
+{
+        int x = x2 - x1;
+        int y = y2 - y1;
+
+        return int_sqrt(x*x + y*y);
+}
+
+static int wacom_24hdt_irq(struct wacom_wac *wacom)
+{
+        struct input_dev *input = wacom->input;
+        char *data = wacom->data;
+        int i;
+        int current_num_contacts = data[61];
+        int contacts_to_send = 0;
+
+        /*
+         * First packet resets the counter since only the first
+         * packet in series will have non-zero current_num_contacts.
+         */
+        if (current_num_contacts)
+                wacom->num_contacts_left = current_num_contacts;
+
+        /* There are at most 4 contacts per packet */
+        contacts_to_send = min(4, wacom->num_contacts_left);
+
+        for (i = 0; i < contacts_to_send; i++) {
+                int offset = (WACOM_BYTES_PER_24HDT_PACKET * i) + 1;
+                bool touch = data[offset] & 0x1 && !wacom->shared->stylus_in_proximity;
+                int id = data[offset + 1];
+                int slot = find_slot_from_contactid(wacom, id);
+
+                if (slot < 0)
+                        continue;
+                input_mt_slot(input, slot);
+                input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
+
+                if (touch) {
+                        int t_x = le16_to_cpup((__le16 *)&data[offset + 2]);
+                        int c_x = le16_to_cpup((__le16 *)&data[offset + 4]);
+                        int t_y = le16_to_cpup((__le16 *)&data[offset + 6]);
+                        int c_y = le16_to_cpup((__le16 *)&data[offset + 8]);
+                        int w = le16_to_cpup((__le16 *)&data[offset + 10]);
+                        int h = le16_to_cpup((__le16 *)&data[offset + 12]);
+
+                        input_report_abs(input, ABS_MT_POSITION_X, t_x);
+                        input_report_abs(input, ABS_MT_POSITION_Y, t_y);
+                        input_report_abs(input, ABS_MT_TOUCH_MAJOR, min(w,h));
+                        input_report_abs(input, ABS_MT_WIDTH_MAJOR, min(w, h) + int_dist(t_x, t_y, c_x, c_y));
+                        input_report_abs(input, ABS_MT_WIDTH_MINOR, min(w, h));
+                        input_report_abs(input, ABS_MT_ORIENTATION, w > h);
+                }
+                wacom->slots[slot] = touch ? id : -1;
+        }
+
+        input_mt_report_pointer_emulation(input, true);
+
+        wacom->num_contacts_left -= contacts_to_send;
+        if (wacom->num_contacts_left <= 0)
+                wacom->num_contacts_left = 0;
+
+        return 1;
+}
+
 static int wacom_mt_touch(struct wacom_wac *wacom)
 {
         struct input_dev *input = wacom->input;
@@ -1043,11 +1112,19 @@ static void wacom_bpt3_touch_msg(struct wacom_wac *wacom, unsigned char *data)
         if (touch) {
                 int x = (data[2] << 4) | (data[4] >> 4);
                 int y = (data[3] << 4) | (data[4] & 0x0f);
-                int w = data[6];
+                int a = data[5];
+
+                // "a" is a scaled-down area which we assume is roughly
+                // circular and which can be described as: a=(pi*r^2)/C.
+                int x_res  = input_abs_get_res(input, ABS_X);
+                int y_res  = input_abs_get_res(input, ABS_Y);
+                int width  = 2 * int_sqrt(a * WACOM_CONTACT_AREA_SCALE);
+                int height = width * y_res / x_res;
 
                 input_report_abs(input, ABS_MT_POSITION_X, x);
                 input_report_abs(input, ABS_MT_POSITION_Y, y);
-                input_report_abs(input, ABS_MT_TOUCH_MAJOR, w);
+                input_report_abs(input, ABS_MT_TOUCH_MAJOR, width);
+                input_report_abs(input, ABS_MT_TOUCH_MINOR, height);
         }
 }
 
@@ -1242,6 +1319,10 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
                 sync = wacom_intuos_irq(wacom_wac);
                 break;
 
+        case WACOM_24HDT:
+                sync = wacom_24hdt_irq(wacom_wac);
+                break;
+
         case INTUOS5S:
         case INTUOS5:
         case INTUOS5L:
@@ -1327,7 +1408,8 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 
         /* these device have multiple inputs */
         if (features->type >= WIRELESS ||
-            (features->type >= INTUOS5S && features->type <= INTUOS5L))
+            (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
+            (features->oVid && features->oPid))
                 features->quirks |= WACOM_QUIRK_MULTI_INPUT;
 
         /* quirk for bamboo touch with 2 low res touches */
@@ -1436,6 +1518,9 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
                 input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
                 input_set_abs_params(input_dev, ABS_THROTTLE, 0, 71, 0, 0);
+
+                __set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
+
                 wacom_setup_cintiq(wacom_wac);
                 break;
 
@@ -1533,7 +1618,9 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
                         input_mt_init_slots(input_dev, features->touch_max, 0);
 
                         input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
-                                             0, 255, 0, 0);
+                                             0, features->x_max, 0, 0);
+                        input_set_abs_params(input_dev, ABS_MT_TOUCH_MINOR,
+                                             0, features->y_max, 0, 0);
 
                         input_set_abs_params(input_dev, ABS_MT_POSITION_X,
                                              0, features->x_max,
@@ -1560,6 +1647,15 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
                 __set_bit(INPUT_PROP_POINTER, input_dev->propbit);
                 break;
 
+        case WACOM_24HDT:
+                if (features->device_type == BTN_TOOL_FINGER) {
+                        input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0, features->x_max, 0, 0);
+                        input_set_abs_params(input_dev, ABS_MT_WIDTH_MAJOR, 0, features->x_max, 0, 0);
+                        input_set_abs_params(input_dev, ABS_MT_WIDTH_MINOR, 0, features->y_max, 0, 0);
+                        input_set_abs_params(input_dev, ABS_MT_ORIENTATION, 0, 1, 0, 0);
+                }
+                /* fall through */
+
         case MTSCREEN:
                 if (features->device_type == BTN_TOOL_FINGER) {
                         wacom_wac->slots = kmalloc(features->touch_max *
@@ -1641,7 +1737,10 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
                                 input_set_abs_params(input_dev,
                                                      ABS_MT_TOUCH_MAJOR,
-                                                     0, 255, 0, 0);
+                                                     0, features->x_max, 0, 0);
+                                input_set_abs_params(input_dev,
+                                                     ABS_MT_TOUCH_MINOR,
+                                                     0, features->y_max, 0, 0);
                         }
 
                         input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1851,8 +1950,11 @@ static const struct wacom_features wacom_features_0xF4 =
         { "Wacom Cintiq 24HD",       WACOM_PKGLEN_INTUOS,   104480, 65600, 2047,
           63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0xF8 =
-        { "Wacom Cintiq 24HD touch", WACOM_PKGLEN_INTUOS,   104480, 65600, 2047,
-          63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+        { "Wacom Cintiq 24HD touch", WACOM_PKGLEN_INTUOS,   104480, 65600, 2047, /* Pen */
+          63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, .oVid = USB_VENDOR_ID_WACOM, .oPid = 0xf6 };
+static const struct wacom_features wacom_features_0xF6 =
+        { "Wacom Cintiq 24HD touch", .type = WACOM_24HDT, /* Touch */
+          .oVid = USB_VENDOR_ID_WACOM, .oPid = 0xf8, .touch_max = 10 };
 static const struct wacom_features wacom_features_0x3F =
         { "Wacom Cintiq 21UX",    WACOM_PKGLEN_INTUOS,    87200, 65600, 1023,
           63, CINTIQ, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
@@ -2095,6 +2197,7 @@ const struct usb_device_id wacom_ids[] = {
         { USB_DEVICE_WACOM(0x47) },
         { USB_DEVICE_WACOM(0xF4) },
         { USB_DEVICE_WACOM(0xF8) },
+        { USB_DEVICE_WACOM(0xF6) },
         { USB_DEVICE_WACOM(0xFA) },
         { USB_DEVICE_LENOVO(0x6004) },
         { }
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index 96c185cc301e..345f1e76975e 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -29,6 +29,7 @@
 
 /* wacom data size per MT contact */
 #define WACOM_BYTES_PER_MT_PACKET       11
+#define WACOM_BYTES_PER_24HDT_PACKET    14
 
 /* device IDs */
 #define STYLUS_DEVICE_ID        0x02
@@ -49,6 +50,7 @@
 #define WACOM_REPORT_TPCHID             15
 #define WACOM_REPORT_TPCST              16
 #define WACOM_REPORT_TPC1FGE            18
+#define WACOM_REPORT_24HDT              1
 
 /* device quirks */
 #define WACOM_QUIRK_MULTI_INPUT         0x0001
@@ -81,6 +83,7 @@ enum {
         WACOM_MO,
         WIRELESS,
         BAMBOO_PT,
+        WACOM_24HDT,
         TABLETPC,   /* add new TPC below */
         TABLETPCE,
         TABLETPC2FG,
@@ -109,6 +112,8 @@ struct wacom_features {
         int distance_fuzz;
         unsigned quirks;
         unsigned touch_max;
+        int oVid;
+        int oPid;
 };
 
 struct wacom_shared {
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 1ba232cbc09d..f7668b24c378 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -239,7 +239,7 @@ config TOUCHSCREEN_EETI
 
 config TOUCHSCREEN_EGALAX
         tristate "EETI eGalax multi-touch panel support"
-        depends on I2C
+        depends on I2C && OF
         help
           Say Y here to enable support for I2C connected EETI
           eGalax multi-touch panels.
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index e92615d0b1b0..1df2396af008 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -320,10 +320,8 @@ static bool mxt_object_writable(unsigned int type)
 static void mxt_dump_message(struct device *dev,
                              struct mxt_message *message)
 {
-        dev_dbg(dev, "reportid: %u\tmessage: %02x %02x %02x %02x %02x %02x %02x\n",
-                message->reportid, message->message[0], message->message[1],
-                message->message[2], message->message[3], message->message[4],
-                message->message[5], message->message[6]);
+        dev_dbg(dev, "reportid: %u\tmessage: %*ph\n",
+                message->reportid, 7, message->message);
 }
 
 static int mxt_check_bootloader(struct i2c_client *client,
diff --git a/drivers/input/touchscreen/egalax_ts.c b/drivers/input/touchscreen/egalax_ts.c
index c1e3460f1195..13fa62fdfb0b 100644
--- a/drivers/input/touchscreen/egalax_ts.c
+++ b/drivers/input/touchscreen/egalax_ts.c
@@ -28,6 +28,7 @@
 #include <linux/slab.h>
 #include <linux/bitops.h>
 #include <linux/input/mt.h>
+#include <linux/of_gpio.h>
 
 /*
  * Mouse Mode: some panel may configure the controller to mouse mode,
@@ -122,9 +123,17 @@ static irqreturn_t egalax_ts_interrupt(int irq, void *dev_id)
 /* wake up controller by an falling edge of interrupt gpio.  */
 static int egalax_wake_up_device(struct i2c_client *client)
 {
-        int gpio = irq_to_gpio(client->irq);
+        struct device_node *np = client->dev.of_node;
+        int gpio;
         int ret;
 
+        if (!np)
+                return -ENODEV;
+
+        gpio = of_get_named_gpio(np, "wakeup-gpios", 0);
+        if (!gpio_is_valid(gpio))
+                return -ENODEV;
+
         ret = gpio_request(gpio, "egalax_irq");
         if (ret < 0) {
                 dev_err(&client->dev,
@@ -181,7 +190,11 @@ static int __devinit egalax_ts_probe(struct i2c_client *client,
         ts->input_dev = input_dev;
 
         /* controller may be in sleep, wake it up. */
-        egalax_wake_up_device(client);
+        error = egalax_wake_up_device(client);
+        if (error) {
+                dev_err(&client->dev, "Failed to wake up the controller\n");
+                goto err_free_dev;
+        }
 
         ret = egalax_firmware_version(client);
         if (ret < 0) {
@@ -274,11 +287,17 @@ static int egalax_ts_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(egalax_ts_pm_ops, egalax_ts_suspend, egalax_ts_resume);
 
+static struct of_device_id egalax_ts_dt_ids[] = {
+        { .compatible = "eeti,egalax_ts" },
+        { /* sentinel */ }
+};
+
 static struct i2c_driver egalax_ts_driver = {
         .driver = {
                 .name   = "egalax_ts",
                 .owner  = THIS_MODULE,
                 .pm     = &egalax_ts_pm_ops,
+                .of_match_table = of_match_ptr(egalax_ts_dt_ids),
         },
         .id_table       = egalax_ts_id,
         .probe          = egalax_ts_probe,
diff --git a/drivers/input/touchscreen/tsc40.c b/drivers/input/touchscreen/tsc40.c
index 63209aaa55f0..eb96f168fb9d 100644
--- a/drivers/input/touchscreen/tsc40.c
+++ b/drivers/input/touchscreen/tsc40.c
@@ -107,7 +107,6 @@ static int tsc_connect(struct serio *serio, struct serio_driver *drv)
         __set_bit(BTN_TOUCH, input_dev->keybit);
         input_set_abs_params(ptsc->dev, ABS_X, 0, 0x3ff, 0, 0);
         input_set_abs_params(ptsc->dev, ABS_Y, 0, 0x3ff, 0, 0);
-        input_set_abs_params(ptsc->dev, ABS_PRESSURE, 0, 0, 0, 0);
 
         serio_set_drvdata(serio, ptsc);
 
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 18b0d99bd4d6..81837b0710a9 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -1599,21 +1599,46 @@ static void __init free_on_init_error(void)
 #endif
 }
 
+/* SB IOAPIC is always on this device in AMD systems */
+#define IOAPIC_SB_DEVID         ((0x00 << 8) | PCI_DEVFN(0x14, 0))
+
 static bool __init check_ioapic_information(void)
 {
+        bool ret, has_sb_ioapic;
         int idx;
 
-        for (idx = 0; idx < nr_ioapics; idx++) {
-                int id = mpc_ioapic_id(idx);
+        has_sb_ioapic = false;
+        ret           = false;
 
-                if (get_ioapic_devid(id) < 0) {
-                        pr_err(FW_BUG "AMD-Vi: IO-APIC[%d] not in IVRS table\n", id);
-                        pr_err("AMD-Vi: Disabling interrupt remapping due to BIOS Bug\n");
-                        return false;
+        for (idx = 0; idx < nr_ioapics; idx++) {
+                int devid, id = mpc_ioapic_id(idx);
+
+                devid = get_ioapic_devid(id);
+                if (devid < 0) {
+                        pr_err(FW_BUG "AMD-Vi: IOAPIC[%d] not in IVRS table\n", id);
+                        ret = false;
+                } else if (devid == IOAPIC_SB_DEVID) {
+                        has_sb_ioapic = true;
+                        ret           = true;
                 }
         }
 
-        return true;
+        if (!has_sb_ioapic) {
+                /*
+                 * We expect the SB IOAPIC to be listed in the IVRS
+                 * table. The system timer is connected to the SB IOAPIC
+                 * and if we don't have it in the list the system will
+                 * panic at boot time.  This situation usually happens
+                 * when the BIOS is buggy and provides us the wrong
+                 * device id for the IOAPIC in the system.
+                 */
+                pr_err(FW_BUG "AMD-Vi: No southbridge IOAPIC found in IVRS table\n");
+        }
+
+        if (!ret)
+                pr_err("AMD-Vi: Disabling interrupt remapping due to BIOS Bug(s)\n");
+
+        return ret;
 }
 
 static void __init free_dma_resources(void)
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index 0b4d62e0c645..a649f146d17b 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -200,7 +200,7 @@ enum {
 
 #define SMMU_ADDR_TO_PFN(addr)  ((addr) >> 12)
 #define SMMU_ADDR_TO_PDN(addr)  ((addr) >> 22)
-#define SMMU_PDN_TO_ADDR(addr)  ((pdn) << 22)
+#define SMMU_PDN_TO_ADDR(pdn)   ((pdn) << 22)
 
 #define _READABLE       (1 << SMMU_PTB_DATA_ASID_READABLE_SHIFT)
 #define _WRITABLE       (1 << SMMU_PTB_DATA_ASID_WRITABLE_SHIFT)
diff --git a/drivers/isdn/gigaset/bas-gigaset.c b/drivers/isdn/gigaset/bas-gigaset.c
index 527588708948..c44950d3eb7b 100644
--- a/drivers/isdn/gigaset/bas-gigaset.c
+++ b/drivers/isdn/gigaset/bas-gigaset.c
@@ -617,7 +617,13 @@ static void int_in_work(struct work_struct *work)
         if (rc == 0)
                 /* success, resubmit interrupt read URB */
                 rc = usb_submit_urb(urb, GFP_ATOMIC);
-        if (rc != 0 && rc != -ENODEV) {
+
+        switch (rc) {
+        case 0:         /* success */
+        case -ENODEV:   /* device gone */
+        case -EINVAL:   /* URB already resubmitted, or terminal badness */
+                break;
+        default:        /* failure: try to recover by resetting the device */
                 dev_err(cs->dev, "clear halt failed: %s\n", get_usb_rcmsg(rc));
                 rc = usb_lock_device_for_reset(ucs->udev, ucs->interface);
                 if (rc == 0) {
@@ -2442,7 +2448,9 @@ static void gigaset_disconnect(struct usb_interface *interface)
 }
 
 /* gigaset_suspend
- * This function is called before the USB connection is suspended.
+ * This function is called before the USB connection is suspended
+ * or before the USB device is reset.
+ * In the latter case, message == PMSG_ON.
  */
 static int gigaset_suspend(struct usb_interface *intf, pm_message_t message)
 {
@@ -2498,7 +2506,12 @@ static int gigaset_suspend(struct usb_interface *intf, pm_message_t message)
         del_timer_sync(&ucs->timer_atrdy);
         del_timer_sync(&ucs->timer_cmd_in);
         del_timer_sync(&ucs->timer_int_in);
-        cancel_work_sync(&ucs->int_in_wq);
+
+        /* don't try to cancel int_in_wq from within reset as it
+         * might be the one requesting the reset
+         */
+        if (message.event != PM_EVENT_ON)
+                cancel_work_sync(&ucs->int_in_wq);
 
         gig_dbg(DEBUG_SUSPEND, "suspend complete");
         return 0;
diff --git a/drivers/isdn/i4l/isdn_ppp.c b/drivers/isdn/i4l/isdn_ppp.c
index a1e760150821..61d78fa03b1a 100644
--- a/drivers/isdn/i4l/isdn_ppp.c
+++ b/drivers/isdn/i4l/isdn_ppp.c
@@ -595,7 +595,7 @@ isdn_ppp_ioctl(int min, struct file *file, unsigned int cmd, unsigned long arg)
                         j = ipc->num / (sizeof(long) * 8);
                         i = ipc->num % (sizeof(long) * 8);
                         if (j < 8)
-                                protos[j] |= (0x1 << i);
+                                protos[j] |= (1UL << i);
                         ipc = ipc->next;
                 }
                 if ((r = set_arg(argp, protos, 8 * sizeof(long))))
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 16578d3b52bb..f508defc0d96 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -63,6 +63,17 @@ config LEDS_LM3533
           hardware-accelerated blinking with maximum on and off periods of 9.8
           and 77 seconds respectively.
 
+config LEDS_LM3642
+        tristate "LED support for LM3642 Chip"
+        depends on LEDS_CLASS && I2C
+        select REGMAP_I2C
+        help
+          This option enables support for LEDs connected to LM3642.
+          The LM3642 is a 4MHz fixed-frequency synchronous boost
+          converter plus 1.5A constant current driver for a high-current
+          white LED.
+
+
 config LEDS_LOCOMO
         tristate "LED Support for Locomo device"
         depends on LEDS_CLASS
@@ -192,11 +203,12 @@ config LEDS_LP5521
           programming the engines.
 
 config LEDS_LP5523
-        tristate "LED Support for N.S. LP5523 LED driver chip"
+        tristate "LED Support for TI/National LP5523/55231 LED driver chip"
         depends on LEDS_CLASS && I2C
         help
-          If you say yes here you get support for the National Semiconductor
-          LP5523 LED driver. It is 9 channel chip with programmable engines.
+          If you say yes here you get support for TI/National Semiconductor
+          LP5523/55231 LED driver.
+          It is 9 channel chip with programmable engines.
           Driver provides direct control via LED class and interface for
           programming the engines.
 
@@ -422,13 +434,13 @@ config LEDS_MAX8997
           This option enables support for on-chip LED drivers on
           MAXIM MAX8997 PMIC.
 
-config LEDS_LM3556
-        tristate "LED support for LM3556 Chip"
+config LEDS_LM355x
+        tristate "LED support for LM355x Chips, LM3554 and LM3556"
         depends on LEDS_CLASS && I2C
         select REGMAP_I2C
         help
-          This option enables support for LEDs connected to LM3556.
-          LM3556 includes Torch, Flash and Indicator functions.
+          This option enables support for LEDs connected to LM355x.
+          LM355x includes Torch, Flash and Indicator functions.
 
 config LEDS_OT200
         tristate "LED support for the Bachmann OT200"
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index a9b627c4f8ba..3fb9641b6194 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_LEDS_BD2802)		+= leds-bd2802.o
 obj-$(CONFIG_LEDS_LOCOMO)               += leds-locomo.o
 obj-$(CONFIG_LEDS_LM3530)               += leds-lm3530.o
 obj-$(CONFIG_LEDS_LM3533)               += leds-lm3533.o
+obj-$(CONFIG_LEDS_LM3642)               += leds-lm3642.o
 obj-$(CONFIG_LEDS_MIKROTIK_RB532)       += leds-rb532.o
 obj-$(CONFIG_LEDS_S3C24XX)              += leds-s3c24xx.o
 obj-$(CONFIG_LEDS_NET48XX)              += leds-net48xx.o
@@ -48,7 +49,7 @@ obj-$(CONFIG_LEDS_NETXBIG)		+= leds-netxbig.o
 obj-$(CONFIG_LEDS_ASIC3)                += leds-asic3.o
 obj-$(CONFIG_LEDS_RENESAS_TPU)          += leds-renesas-tpu.o
 obj-$(CONFIG_LEDS_MAX8997)              += leds-max8997.o
-obj-$(CONFIG_LEDS_LM3556)               += leds-lm3556.o
+obj-$(CONFIG_LEDS_LM355x)               += leds-lm355x.o
 obj-$(CONFIG_LEDS_BLINKM)               += leds-blinkm.o
 
 # LED SPI Drivers
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index c599095bc005..48cce18e9d6d 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -124,6 +124,16 @@ static void led_timer_function(unsigned long data)
         mod_timer(&led_cdev->blink_timer, jiffies + msecs_to_jiffies(delay));
 }
 
+static void set_brightness_delayed(struct work_struct *ws)
+{
+        struct led_classdev *led_cdev =
+                container_of(ws, struct led_classdev, set_brightness_work);
+
+        led_stop_software_blink(led_cdev);
+
+        __led_set_brightness(led_cdev, led_cdev->delayed_set_value);
+}
+
 /**
  * led_classdev_suspend - suspend an led_classdev.
  * @led_cdev: the led_classdev to suspend.
@@ -191,6 +201,8 @@ int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 
         led_update_brightness(led_cdev);
 
+        INIT_WORK(&led_cdev->set_brightness_work, set_brightness_delayed);
+
         init_timer(&led_cdev->blink_timer);
         led_cdev->blink_timer.function = led_timer_function;
         led_cdev->blink_timer.data = (unsigned long)led_cdev;
@@ -221,7 +233,10 @@ void led_classdev_unregister(struct led_classdev *led_cdev)
         up_write(&led_cdev->trigger_lock);
 #endif
 
+        cancel_work_sync(&led_cdev->set_brightness_work);
+
         /* Stop blinking */
+        led_stop_software_blink(led_cdev);
         led_set_brightness(led_cdev, LED_OFF);
 
         device_unregister(led_cdev->dev);
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index 2ab05af3de31..ce8921a753a3 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -103,13 +103,23 @@ void led_blink_set_oneshot(struct led_classdev *led_cdev,
 }
 EXPORT_SYMBOL(led_blink_set_oneshot);
 
-void led_set_brightness(struct led_classdev *led_cdev,
-                        enum led_brightness brightness)
+void led_stop_software_blink(struct led_classdev *led_cdev)
 {
-        /* stop and clear soft-blink timer */
         del_timer_sync(&led_cdev->blink_timer);
         led_cdev->blink_delay_on = 0;
         led_cdev->blink_delay_off = 0;
+}
+EXPORT_SYMBOL_GPL(led_stop_software_blink);
+
+void led_set_brightness(struct led_classdev *led_cdev,
+                        enum led_brightness brightness)
+{
+        /* delay brightness setting if need to stop soft-blink timer */
+        if (led_cdev->blink_delay_on || led_cdev->blink_delay_off) {
+                led_cdev->delayed_set_value = brightness;
+                schedule_work(&led_cdev->set_brightness_work);
+                return;
+        }
 
         __led_set_brightness(led_cdev, brightness);
 }
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index 363975b3c925..262eb4193710 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -102,6 +102,12 @@ EXPORT_SYMBOL_GPL(led_trigger_show);
 void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 {
         unsigned long flags;
+        char *event = NULL;
+        char *envp[2];
+        const char *name;
+
+        name = trig ? trig->name : "none";
+        event = kasprintf(GFP_KERNEL, "TRIGGER=%s", name);
 
         /* Remove any existing trigger */
         if (led_cdev->trigger) {
@@ -109,6 +115,8 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
                 list_del(&led_cdev->trig_list);
                 write_unlock_irqrestore(&led_cdev->trigger->leddev_list_lock,
                         flags);
+                cancel_work_sync(&led_cdev->set_brightness_work);
+                led_stop_software_blink(led_cdev);
                 if (led_cdev->trigger->deactivate)
                         led_cdev->trigger->deactivate(led_cdev);
                 led_cdev->trigger = NULL;
@@ -122,6 +130,13 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
                 if (trig->activate)
                         trig->activate(led_cdev);
         }
+
+        if (event) {
+                envp[0] = event;
+                envp[1] = NULL;
+                kobject_uevent_env(&led_cdev->dev->kobj, KOBJ_CHANGE, envp);
+                kfree(event);
+        }
 }
 EXPORT_SYMBOL_GPL(led_trigger_set);
 
@@ -224,7 +239,7 @@ void led_trigger_event(struct led_trigger *trig,
                 struct led_classdev *led_cdev;
 
                 led_cdev = list_entry(entry, struct led_classdev, trig_list);
-                __led_set_brightness(led_cdev, brightness);
+                led_set_brightness(led_cdev, brightness);
         }
         read_unlock(&trig->leddev_list_lock);
 }
diff --git a/drivers/leds/leds-clevo-mail.c b/drivers/leds/leds-clevo-mail.c
index 1ed1677c916f..e024b0b1c3b1 100644
--- a/drivers/leds/leds-clevo-mail.c
+++ b/drivers/leds/leds-clevo-mail.c
@@ -31,7 +31,7 @@ static int __init clevo_mail_led_dmi_callback(const struct dmi_system_id *id)
 }
 
 /*
- * struct mail_led_whitelist - List of known good models
+ * struct clevo_mail_led_dmi_table - List of known good models
  *
  * Contains the known good models this driver is compatible with.
  * When adding a new model try to be as strict as possible. This
@@ -39,7 +39,7 @@ static int __init clevo_mail_led_dmi_callback(const struct dmi_system_id *id)
  * detected as working, but in reality it is not) as low as
  * possible.
  */
-static struct dmi_system_id __initdata mail_led_whitelist[] = {
+static struct dmi_system_id __initdata clevo_mail_led_dmi_table[] = {
         {
                 .callback = clevo_mail_led_dmi_callback,
                 .ident = "Clevo D410J",
@@ -59,11 +59,10 @@ static struct dmi_system_id __initdata mail_led_whitelist[] = {
         },
         {
                 .callback = clevo_mail_led_dmi_callback,
-                .ident = "Positivo Mobile",
+                .ident = "Clevo M5x0V",
                 .matches = {
                         DMI_MATCH(DMI_BOARD_VENDOR, "CLEVO Co. "),
                         DMI_MATCH(DMI_BOARD_NAME, "M5X0V "),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "Positivo Mobile"),
                         DMI_MATCH(DMI_PRODUCT_VERSION, "VT6198")
                 }
         },
@@ -89,6 +88,7 @@ static struct dmi_system_id __initdata mail_led_whitelist[] = {
         },
         { }
 };
+MODULE_DEVICE_TABLE(dmi, clevo_mail_led_dmi_table);
 
 static void clevo_mail_led_set(struct led_classdev *led_cdev,
                                 enum led_brightness value)
@@ -180,7 +180,7 @@ static int __init clevo_mail_led_init(void)
 
         /* Check with the help of DMI if we are running on supported hardware */
         if (!nodetect) {
-                count = dmi_check_system(mail_led_whitelist);
+                count = dmi_check_system(clevo_mail_led_dmi_table);
         } else {
                 count = 1;
                 printk(KERN_ERR KBUILD_MODNAME ": Skipping DMI detection. "
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index c032b2180340..087d1e66f4f7 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -20,6 +20,7 @@
 #include <linux/slab.h>
 #include <linux/workqueue.h>
 #include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
 
 struct gpio_led_data {
         struct led_classdev cdev;
@@ -170,11 +171,10 @@ static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_dev
 {
         struct device_node *np = pdev->dev.of_node, *child;
         struct gpio_leds_priv *priv;
-        int count = 0, ret;
+        int count, ret;
 
         /* count LEDs in this device, so we know how much to allocate */
-        for_each_child_of_node(np, child)
-                count++;
+        count = of_get_child_count(np);
         if (!count)
                 return NULL;
 
@@ -228,7 +228,6 @@ static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_dev
 {
         return NULL;
 }
-#define of_gpio_leds_match NULL
 #endif /* CONFIG_OF_GPIO */
 
 
@@ -236,8 +235,14 @@ static int __devinit gpio_led_probe(struct platform_device *pdev)
 {
         struct gpio_led_platform_data *pdata = pdev->dev.platform_data;
         struct gpio_leds_priv *priv;
+        struct pinctrl *pinctrl;
         int i, ret = 0;
 
+        pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+        if (IS_ERR(pinctrl))
+                dev_warn(&pdev->dev,
+                        "pins are not configured from the driver\n");
+
         if (pdata && pdata->num_leds) {
                 priv = devm_kzalloc(&pdev->dev,
                                 sizeof_gpio_leds_priv(pdata->num_leds),
@@ -270,13 +275,13 @@ static int __devinit gpio_led_probe(struct platform_device *pdev)
 
 static int __devexit gpio_led_remove(struct platform_device *pdev)
 {
-        struct gpio_leds_priv *priv = dev_get_drvdata(&pdev->dev);
+        struct gpio_leds_priv *priv = platform_get_drvdata(pdev);
         int i;
 
         for (i = 0; i < priv->num_leds; i++)
                 delete_gpio_led(&priv->leds[i]);
 
-        dev_set_drvdata(&pdev->dev, NULL);
+        platform_set_drvdata(pdev, NULL);
 
         return 0;
 }
@@ -287,7 +292,7 @@ static struct platform_driver gpio_led_driver = {
         .driver         = {
                 .name   = "leds-gpio",
                 .owner  = THIS_MODULE,
-                .of_match_table = of_gpio_leds_match,
+                .of_match_table = of_match_ptr(of_gpio_leds_match),
         },
 };
 
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 23637bdb275d..b26306f6724d 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -150,7 +150,7 @@ static int lm3530_get_mode_from_str(const char *str)
                 if (sysfs_streq(str, mode_map[i].mode))
                         return mode_map[i].mode_val;
 
-        return -1;
+        return -EINVAL;
 }
 
 static void lm3530_als_configure(struct lm3530_platform_data *pdata,
@@ -358,7 +358,7 @@ static ssize_t lm3530_mode_set(struct device *dev, struct device_attribute
         mode = lm3530_get_mode_from_str(buf);
         if (mode < 0) {
                 dev_err(dev, "Invalid mode\n");
-                return -EINVAL;
+                return mode;
         }
 
         drvdata->mode = mode;
@@ -416,7 +416,7 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 
         i2c_set_clientdata(client, drvdata);
 
-        drvdata->regulator = regulator_get(&client->dev, "vin");
+        drvdata->regulator = devm_regulator_get(&client->dev, "vin");
         if (IS_ERR(drvdata->regulator)) {
                 dev_err(&client->dev, "regulator get failed\n");
                 err = PTR_ERR(drvdata->regulator);
@@ -429,15 +429,13 @@ static int __devinit lm3530_probe(struct i2c_client *client,
                 if (err < 0) {
                         dev_err(&client->dev,
                                 "Register Init failed: %d\n", err);
-                        err = -ENODEV;
-                        goto err_reg_init;
+                        return err;
                 }
         }
         err = led_classdev_register(&client->dev, &drvdata->led_dev);
         if (err < 0) {
                 dev_err(&client->dev, "Register led class failed: %d\n", err);
-                err = -ENODEV;
-                goto err_class_register;
+                return err;
         }
 
         err = device_create_file(drvdata->led_dev.dev, &dev_attr_mode);
@@ -451,9 +449,6 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 
 err_create_file:
         led_classdev_unregister(&drvdata->led_dev);
-err_class_register:
-err_reg_init:
-        regulator_put(drvdata->regulator);
         return err;
 }
 
@@ -465,7 +460,6 @@ static int __devexit lm3530_remove(struct i2c_client *client)
 
         if (drvdata->enable)
                 regulator_disable(drvdata->regulator);
-        regulator_put(drvdata->regulator);
         led_classdev_unregister(&drvdata->led_dev);
         return 0;
 }
diff --git a/drivers/leds/leds-lm3556.c b/drivers/leds/leds-lm3556.c
deleted file mode 100644
index 3062abd9a532..000000000000
--- a/drivers/leds/leds-lm3556.c
+++ /dev/null
@@ -1,512 +0,0 @@
-/*
- * Simple driver for Texas Instruments LM3556 LED Flash driver chip (Rev0x03)
- * Copyright (C) 2012 Texas Instruments
- *
- * 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.
- *
- * Please refer Documentation/leds/leds-lm3556.txt file.
- */
-#include <linux/module.h>
-#include <linux/delay.h>
-#include <linux/i2c.h>
-#include <linux/leds.h>
-#include <linux/slab.h>
-#include <linux/platform_device.h>
-#include <linux/fs.h>
-#include <linux/regmap.h>
-#include <linux/platform_data/leds-lm3556.h>
-
-#define REG_FILT_TIME                   (0x0)
-#define REG_IVFM_MODE                   (0x1)
-#define REG_NTC                         (0x2)
-#define REG_INDIC_TIME                  (0x3)
-#define REG_INDIC_BLINK                 (0x4)
-#define REG_INDIC_PERIOD                (0x5)
-#define REG_TORCH_TIME                  (0x6)
-#define REG_CONF                        (0x7)
-#define REG_FLASH                       (0x8)
-#define REG_I_CTRL                      (0x9)
-#define REG_ENABLE                      (0xA)
-#define REG_FLAG                        (0xB)
-#define REG_MAX                         (0xB)
-
-#define IVFM_FILTER_TIME_SHIFT          (3)
-#define UVLO_EN_SHIFT                   (7)
-#define HYSTERSIS_SHIFT                 (5)
-#define IVM_D_TH_SHIFT                  (2)
-#define IVFM_ADJ_MODE_SHIFT             (0)
-#define NTC_EVENT_LVL_SHIFT             (5)
-#define NTC_TRIP_TH_SHIFT               (2)
-#define NTC_BIAS_I_LVL_SHIFT            (0)
-#define INDIC_RAMP_UP_TIME_SHIFT        (3)
-#define INDIC_RAMP_DN_TIME_SHIFT        (0)
-#define INDIC_N_BLANK_SHIFT             (4)
-#define INDIC_PULSE_TIME_SHIFT          (0)
-#define INDIC_N_PERIOD_SHIFT            (0)
-#define TORCH_RAMP_UP_TIME_SHIFT        (3)
-#define TORCH_RAMP_DN_TIME_SHIFT        (0)
-#define STROBE_USUAGE_SHIFT             (7)
-#define STROBE_PIN_POLARITY_SHIFT       (6)
-#define TORCH_PIN_POLARITY_SHIFT        (5)
-#define TX_PIN_POLARITY_SHIFT           (4)
-#define TX_EVENT_LVL_SHIFT              (3)
-#define IVFM_EN_SHIFT                   (2)
-#define NTC_MODE_SHIFT                  (1)
-#define INDIC_MODE_SHIFT                (0)
-#define INDUCTOR_I_LIMIT_SHIFT          (6)
-#define FLASH_RAMP_TIME_SHIFT           (3)
-#define FLASH_TOUT_TIME_SHIFT           (0)
-#define TORCH_I_SHIFT                   (4)
-#define FLASH_I_SHIFT                   (0)
-#define NTC_EN_SHIFT                    (7)
-#define TX_PIN_EN_SHIFT                 (6)
-#define STROBE_PIN_EN_SHIFT             (5)
-#define TORCH_PIN_EN_SHIFT              (4)
-#define PRECHG_MODE_EN_SHIFT            (3)
-#define PASS_MODE_ONLY_EN_SHIFT         (2)
-#define MODE_BITS_SHIFT                 (0)
-
-#define IVFM_FILTER_TIME_MASK           (0x3)
-#define UVLO_EN_MASK                    (0x1)
-#define HYSTERSIS_MASK                  (0x3)
-#define IVM_D_TH_MASK                   (0x7)
-#define IVFM_ADJ_MODE_MASK              (0x3)
-#define NTC_EVENT_LVL_MASK              (0x1)
-#define NTC_TRIP_TH_MASK                (0x7)
-#define NTC_BIAS_I_LVL_MASK             (0x3)
-#define INDIC_RAMP_UP_TIME_MASK         (0x7)
-#define INDIC_RAMP_DN_TIME_MASK         (0x7)
-#define INDIC_N_BLANK_MASK              (0x7)
-#define INDIC_PULSE_TIME_MASK           (0x7)
-#define INDIC_N_PERIOD_MASK             (0x7)
-#define TORCH_RAMP_UP_TIME_MASK         (0x7)
-#define TORCH_RAMP_DN_TIME_MASK         (0x7)
-#define STROBE_USUAGE_MASK              (0x1)
-#define STROBE_PIN_POLARITY_MASK        (0x1)
-#define TORCH_PIN_POLARITY_MASK         (0x1)
-#define TX_PIN_POLARITY_MASK            (0x1)
-#define TX_EVENT_LVL_MASK               (0x1)
-#define IVFM_EN_MASK                    (0x1)
-#define NTC_MODE_MASK                   (0x1)
-#define INDIC_MODE_MASK                 (0x1)
-#define INDUCTOR_I_LIMIT_MASK           (0x3)
-#define FLASH_RAMP_TIME_MASK            (0x7)
-#define FLASH_TOUT_TIME_MASK            (0x7)
-#define TORCH_I_MASK                    (0x7)
-#define FLASH_I_MASK                    (0xF)
-#define NTC_EN_MASK                     (0x1)
-#define TX_PIN_EN_MASK                  (0x1)
-#define STROBE_PIN_EN_MASK              (0x1)
-#define TORCH_PIN_EN_MASK               (0x1)
-#define PRECHG_MODE_EN_MASK             (0x1)
-#define PASS_MODE_ONLY_EN_MASK          (0x1)
-#define MODE_BITS_MASK                  (0x13)
-#define EX_PIN_CONTROL_MASK             (0xF1)
-#define EX_PIN_ENABLE_MASK              (0x70)
-
-enum lm3556_indic_pulse_time {
-        PULSE_TIME_0_MS = 0,
-        PULSE_TIME_32_MS,
-        PULSE_TIME_64_MS,
-        PULSE_TIME_92_MS,
-        PULSE_TIME_128_MS,
-        PULSE_TIME_160_MS,
-        PULSE_TIME_196_MS,
-        PULSE_TIME_224_MS,
-        PULSE_TIME_256_MS,
-        PULSE_TIME_288_MS,
-        PULSE_TIME_320_MS,
-        PULSE_TIME_352_MS,
-        PULSE_TIME_384_MS,
-        PULSE_TIME_416_MS,
-        PULSE_TIME_448_MS,
-        PULSE_TIME_480_MS,
-};
-
-enum lm3556_indic_n_blank {
-        INDIC_N_BLANK_0 = 0,
-        INDIC_N_BLANK_1,
-        INDIC_N_BLANK_2,
-        INDIC_N_BLANK_3,
-        INDIC_N_BLANK_4,
-        INDIC_N_BLANK_5,
-        INDIC_N_BLANK_6,
-        INDIC_N_BLANK_7,
-        INDIC_N_BLANK_8,
-        INDIC_N_BLANK_9,
-        INDIC_N_BLANK_10,
-        INDIC_N_BLANK_11,
-        INDIC_N_BLANK_12,
-        INDIC_N_BLANK_13,
-        INDIC_N_BLANK_14,
-        INDIC_N_BLANK_15,
-};
-
-enum lm3556_indic_period {
-        INDIC_PERIOD_0 = 0,
-        INDIC_PERIOD_1,
-        INDIC_PERIOD_2,
-        INDIC_PERIOD_3,
-        INDIC_PERIOD_4,
-        INDIC_PERIOD_5,
-        INDIC_PERIOD_6,
-        INDIC_PERIOD_7,
-};
-
-enum lm3556_mode {
-        MODES_STASNDBY = 0,
-        MODES_INDIC,
-        MODES_TORCH,
-        MODES_FLASH
-};
-
-#define INDIC_PATTERN_SIZE 4
-
-struct indicator {
-        u8 blinking;
-        u8 period_cnt;
-};
-
-struct lm3556_chip_data {
-        struct device *dev;
-
-        struct led_classdev cdev_flash;
-        struct led_classdev cdev_torch;
-        struct led_classdev cdev_indicator;
-
-        struct lm3556_platform_data *pdata;
-        struct regmap *regmap;
-        struct mutex lock;
-
-        unsigned int last_flag;
-};
-
-/* indicator pattern */
-static struct indicator indicator_pattern[INDIC_PATTERN_SIZE] = {
-        [0] = {(INDIC_N_BLANK_1 << INDIC_N_BLANK_SHIFT)
-               | PULSE_TIME_32_MS, INDIC_PERIOD_1},
-        [1] = {(INDIC_N_BLANK_15 << INDIC_N_BLANK_SHIFT)
-               | PULSE_TIME_32_MS, INDIC_PERIOD_2},
-        [2] = {(INDIC_N_BLANK_10 << INDIC_N_BLANK_SHIFT)
-               | PULSE_TIME_32_MS, INDIC_PERIOD_4},
-        [3] = {(INDIC_N_BLANK_5 << INDIC_N_BLANK_SHIFT)
-               | PULSE_TIME_32_MS, INDIC_PERIOD_7},
-};
-
-/* chip initialize */
-static int __devinit lm3556_chip_init(struct lm3556_chip_data *chip)
-{
-        unsigned int reg_val;
-        int ret;
-        struct lm3556_platform_data *pdata = chip->pdata;
-
-        /* set config register */
-        ret = regmap_read(chip->regmap, REG_CONF, &reg_val);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to read REG_CONF Register\n");
-                goto out;
-        }
-
-        reg_val &= (~EX_PIN_CONTROL_MASK);
-        reg_val |= ((pdata->torch_pin_polarity & 0x01)
-                    << TORCH_PIN_POLARITY_SHIFT);
-        reg_val |= ((pdata->strobe_usuage & 0x01) << STROBE_USUAGE_SHIFT);
-        reg_val |= ((pdata->strobe_pin_polarity & 0x01)
-                    << STROBE_PIN_POLARITY_SHIFT);
-        reg_val |= ((pdata->tx_pin_polarity & 0x01) << TX_PIN_POLARITY_SHIFT);
-        reg_val |= ((pdata->indicator_mode & 0x01) << INDIC_MODE_SHIFT);
-
-        ret = regmap_write(chip->regmap, REG_CONF, reg_val);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to write REG_CONF Regisgter\n");
-                goto out;
-        }
-
-        /* set enable register */
-        ret = regmap_read(chip->regmap, REG_ENABLE, &reg_val);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to read REG_ENABLE Register\n");
-                goto out;
-        }
-
-        reg_val &= (~EX_PIN_ENABLE_MASK);
-        reg_val |= ((pdata->torch_pin_en & 0x01) << TORCH_PIN_EN_SHIFT);
-        reg_val |= ((pdata->strobe_pin_en & 0x01) << STROBE_PIN_EN_SHIFT);
-        reg_val |= ((pdata->tx_pin_en & 0x01) << TX_PIN_EN_SHIFT);
-
-        ret = regmap_write(chip->regmap, REG_ENABLE, reg_val);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
-                goto out;
-        }
-
-out:
-        return ret;
-}
-
-/* chip control */
-static int lm3556_control(struct lm3556_chip_data *chip,
-                          u8 brightness, enum lm3556_mode opmode)
-{
-        int ret;
-        struct lm3556_platform_data *pdata = chip->pdata;
-
-        ret = regmap_read(chip->regmap, REG_FLAG, &chip->last_flag);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to read REG_FLAG Register\n");
-                goto out;
-        }
-
-        if (chip->last_flag)
-                dev_info(chip->dev, "Last FLAG is 0x%x\n", chip->last_flag);
-
-        /* brightness 0 means off-state */
-        if (!brightness)
-                opmode = MODES_STASNDBY;
-
-        switch (opmode) {
-        case MODES_TORCH:
-                ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
-                                         TORCH_I_MASK << TORCH_I_SHIFT,
-                                         (brightness - 1) << TORCH_I_SHIFT);
-
-                if (pdata->torch_pin_en)
-                        opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
-                break;
-
-        case MODES_FLASH:
-                ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
-                                         FLASH_I_MASK << FLASH_I_SHIFT,
-                                         (brightness - 1) << FLASH_I_SHIFT);
-                break;
-
-        case MODES_INDIC:
-                ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
-                                         TORCH_I_MASK << TORCH_I_SHIFT,
-                                         (brightness - 1) << TORCH_I_SHIFT);
-                break;
-
-        case MODES_STASNDBY:
-                if (pdata->torch_pin_en)
-                        opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
-                break;
-
-        default:
-                return ret;
-        }
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to write REG_I_CTRL Register\n");
-                goto out;
-        }
-        ret = regmap_update_bits(chip->regmap, REG_ENABLE,
-                                 MODE_BITS_MASK << MODE_BITS_SHIFT,
-                                 opmode << MODE_BITS_SHIFT);
-
-out:
-        return ret;
-}
-
-/* torch */
-static void lm3556_torch_brightness_set(struct led_classdev *cdev,
-                                        enum led_brightness brightness)
-{
-        struct lm3556_chip_data *chip =
-            container_of(cdev, struct lm3556_chip_data, cdev_torch);
-
-        mutex_lock(&chip->lock);
-        lm3556_control(chip, brightness, MODES_TORCH);
-        mutex_unlock(&chip->lock);
-}
-
-/* flash */
-static void lm3556_strobe_brightness_set(struct led_classdev *cdev,
-                                         enum led_brightness brightness)
-{
-        struct lm3556_chip_data *chip =
-            container_of(cdev, struct lm3556_chip_data, cdev_flash);
-
-        mutex_lock(&chip->lock);
-        lm3556_control(chip, brightness, MODES_FLASH);
-        mutex_unlock(&chip->lock);
-}
-
-/* indicator */
-static void lm3556_indicator_brightness_set(struct led_classdev *cdev,
-                                            enum led_brightness brightness)
-{
-        struct lm3556_chip_data *chip =
-            container_of(cdev, struct lm3556_chip_data, cdev_indicator);
-
-        mutex_lock(&chip->lock);
-        lm3556_control(chip, brightness, MODES_INDIC);
-        mutex_unlock(&chip->lock);
-}
-
-/* indicator pattern */
-static ssize_t lm3556_indicator_pattern_store(struct device *dev,
-                                              struct device_attribute *devAttr,
-                                              const char *buf, size_t size)
-{
-        ssize_t ret;
-        struct led_classdev *led_cdev = dev_get_drvdata(dev);
-        struct lm3556_chip_data *chip =
-            container_of(led_cdev, struct lm3556_chip_data, cdev_indicator);
-        unsigned int state;
-
-        ret = kstrtouint(buf, 10, &state);
-        if (ret)
-                goto out;
-        if (state > INDIC_PATTERN_SIZE - 1)
-                state = INDIC_PATTERN_SIZE - 1;
-
-        ret = regmap_write(chip->regmap, REG_INDIC_BLINK,
-                           indicator_pattern[state].blinking);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
-                goto out;
-        }
-
-        ret = regmap_write(chip->regmap, REG_INDIC_PERIOD,
-                           indicator_pattern[state].period_cnt);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
-                goto out;
-        }
-
-        return size;
-out:
-        dev_err(chip->dev, "Indicator pattern doesn't saved\n");
-        return size;
-}
-
-static DEVICE_ATTR(pattern, 0666, NULL, lm3556_indicator_pattern_store);
-
-static const struct regmap_config lm3556_regmap = {
-        .reg_bits = 8,
-        .val_bits = 8,
-        .max_register = REG_MAX,
-};
-
-/* module initialize */
-static int __devinit lm3556_probe(struct i2c_client *client,
-                                  const struct i2c_device_id *id)
-{
-        struct lm3556_platform_data *pdata = client->dev.platform_data;
-        struct lm3556_chip_data *chip;
-
-        int err;
-
-        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-                dev_err(&client->dev, "i2c functionality check fail.\n");
-                return -EOPNOTSUPP;
-        }
-
-        if (pdata == NULL) {
-                dev_err(&client->dev, "Needs Platform Data.\n");
-                return -ENODATA;
-        }
-
-        chip =
-            devm_kzalloc(&client->dev, sizeof(struct lm3556_chip_data),
-                         GFP_KERNEL);
-        if (!chip)
-                return -ENOMEM;
-
-        chip->dev = &client->dev;
-        chip->pdata = pdata;
-
-        chip->regmap = devm_regmap_init_i2c(client, &lm3556_regmap);
-        if (IS_ERR(chip->regmap)) {
-                err = PTR_ERR(chip->regmap);
-                dev_err(&client->dev, "Failed to allocate register map: %d\n",
-                        err);
-                return err;
-        }
-
-        mutex_init(&chip->lock);
-        i2c_set_clientdata(client, chip);
-
-        err = lm3556_chip_init(chip);
-        if (err < 0)
-                goto err_out;
-
-        /* flash */
-        chip->cdev_flash.name = "flash";
-        chip->cdev_flash.max_brightness = 16;
-        chip->cdev_flash.brightness_set = lm3556_strobe_brightness_set;
-        err = led_classdev_register((struct device *)
-                                    &client->dev, &chip->cdev_flash);
-        if (err < 0)
-                goto err_out;
-        /* torch */
-        chip->cdev_torch.name = "torch";
-        chip->cdev_torch.max_brightness = 8;
-        chip->cdev_torch.brightness_set = lm3556_torch_brightness_set;
-        err = led_classdev_register((struct device *)
-                                    &client->dev, &chip->cdev_torch);
-        if (err < 0)
-                goto err_create_torch_file;
-        /* indicator */
-        chip->cdev_indicator.name = "indicator";
-        chip->cdev_indicator.max_brightness = 8;
-        chip->cdev_indicator.brightness_set = lm3556_indicator_brightness_set;
-        err = led_classdev_register((struct device *)
-                                    &client->dev, &chip->cdev_indicator);
-        if (err < 0)
-                goto err_create_indicator_file;
-
-        err = device_create_file(chip->cdev_indicator.dev, &dev_attr_pattern);
-        if (err < 0)
-                goto err_create_pattern_file;
-
-        dev_info(&client->dev, "LM3556 is initialized\n");
-        return 0;
-
-err_create_pattern_file:
-        led_classdev_unregister(&chip->cdev_indicator);
-err_create_indicator_file:
-        led_classdev_unregister(&chip->cdev_torch);
-err_create_torch_file:
-        led_classdev_unregister(&chip->cdev_flash);
-err_out:
-        return err;
-}
-
-static int __devexit lm3556_remove(struct i2c_client *client)
-{
-        struct lm3556_chip_data *chip = i2c_get_clientdata(client);
-
-        device_remove_file(chip->cdev_indicator.dev, &dev_attr_pattern);
-        led_classdev_unregister(&chip->cdev_indicator);
-        led_classdev_unregister(&chip->cdev_torch);
-        led_classdev_unregister(&chip->cdev_flash);
-        regmap_write(chip->regmap, REG_ENABLE, 0);
-        return 0;
-}
-
-static const struct i2c_device_id lm3556_id[] = {
-        {LM3556_NAME, 0},
-        {}
-};
-
-MODULE_DEVICE_TABLE(i2c, lm3556_id);
-
-static struct i2c_driver lm3556_i2c_driver = {
-        .driver = {
-                   .name = LM3556_NAME,
-                   .owner = THIS_MODULE,
-                   .pm = NULL,
-                   },
-        .probe = lm3556_probe,
-        .remove = __devexit_p(lm3556_remove),
-        .id_table = lm3556_id,
-};
-
-module_i2c_driver(lm3556_i2c_driver);
-
-MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM3556");
-MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
-MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lm355x.c b/drivers/leds/leds-lm355x.c
new file mode 100644
index 000000000000..065ec015d67a
--- /dev/null
+++ b/drivers/leds/leds-lm355x.c
@@ -0,0 +1,572 @@
+/*
+* Simple driver for Texas Instruments LM355x LED Flash driver chip
+* Copyright (C) 2012 Texas Instruments
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*/
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/regmap.h>
+#include <linux/workqueue.h>
+#include <linux/platform_data/leds-lm355x.h>
+
+enum lm355x_type {
+        CHIP_LM3554 = 0,
+        CHIP_LM3556,
+};
+
+enum lm355x_regs {
+        REG_FLAG = 0,
+        REG_TORCH_CFG,
+        REG_TORCH_CTRL,
+        REG_STROBE_CFG,
+        REG_FLASH_CTRL,
+        REG_INDI_CFG,
+        REG_INDI_CTRL,
+        REG_OPMODE,
+        REG_MAX,
+};
+
+/* operation mode */
+enum lm355x_mode {
+        MODE_SHDN = 0,
+        MODE_INDIC,
+        MODE_TORCH,
+        MODE_FLASH
+};
+
+/* register map info. */
+struct lm355x_reg_data {
+        u8 regno;
+        u8 mask;
+        u8 shift;
+};
+
+struct lm355x_chip_data {
+        struct device *dev;
+        enum lm355x_type type;
+
+        struct led_classdev cdev_flash;
+        struct led_classdev cdev_torch;
+        struct led_classdev cdev_indicator;
+
+        struct work_struct work_flash;
+        struct work_struct work_torch;
+        struct work_struct work_indicator;
+
+        u8 br_flash;
+        u8 br_torch;
+        u8 br_indicator;
+
+        struct lm355x_platform_data *pdata;
+        struct regmap *regmap;
+        struct mutex lock;
+
+        unsigned int last_flag;
+        struct lm355x_reg_data *regs;
+};
+
+/* specific indicator function for lm3556 */
+enum lm3556_indic_pulse_time {
+        PULSE_TIME_0_MS = 0,
+        PULSE_TIME_32_MS,
+        PULSE_TIME_64_MS,
+        PULSE_TIME_92_MS,
+        PULSE_TIME_128_MS,
+        PULSE_TIME_160_MS,
+        PULSE_TIME_196_MS,
+        PULSE_TIME_224_MS,
+        PULSE_TIME_256_MS,
+        PULSE_TIME_288_MS,
+        PULSE_TIME_320_MS,
+        PULSE_TIME_352_MS,
+        PULSE_TIME_384_MS,
+        PULSE_TIME_416_MS,
+        PULSE_TIME_448_MS,
+        PULSE_TIME_480_MS,
+};
+
+enum lm3556_indic_n_blank {
+        INDIC_N_BLANK_0 = 0,
+        INDIC_N_BLANK_1,
+        INDIC_N_BLANK_2,
+        INDIC_N_BLANK_3,
+        INDIC_N_BLANK_4,
+        INDIC_N_BLANK_5,
+        INDIC_N_BLANK_6,
+        INDIC_N_BLANK_7,
+        INDIC_N_BLANK_8,
+        INDIC_N_BLANK_9,
+        INDIC_N_BLANK_10,
+        INDIC_N_BLANK_11,
+        INDIC_N_BLANK_12,
+        INDIC_N_BLANK_13,
+        INDIC_N_BLANK_14,
+        INDIC_N_BLANK_15,
+};
+
+enum lm3556_indic_period {
+        INDIC_PERIOD_0 = 0,
+        INDIC_PERIOD_1,
+        INDIC_PERIOD_2,
+        INDIC_PERIOD_3,
+        INDIC_PERIOD_4,
+        INDIC_PERIOD_5,
+        INDIC_PERIOD_6,
+        INDIC_PERIOD_7,
+};
+
+#define INDIC_PATTERN_SIZE 4
+
+struct indicator {
+        u8 blinking;
+        u8 period_cnt;
+};
+
+/* indicator pattern data only for lm3556 */
+static struct indicator indicator_pattern[INDIC_PATTERN_SIZE] = {
+        [0] = {(INDIC_N_BLANK_1 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_1},
+        [1] = {(INDIC_N_BLANK_15 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_2},
+        [2] = {(INDIC_N_BLANK_10 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_4},
+        [3] = {(INDIC_N_BLANK_5 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_7},
+};
+
+static struct lm355x_reg_data lm3554_regs[REG_MAX] = {
+        [REG_FLAG] = {0xD0, 0xBF, 0},
+        [REG_TORCH_CFG] = {0xE0, 0x80, 7},
+        [REG_TORCH_CTRL] = {0xA0, 0x38, 3},
+        [REG_STROBE_CFG] = {0xE0, 0x04, 2},
+        [REG_FLASH_CTRL] = {0xB0, 0x78, 3},
+        [REG_INDI_CFG] = {0xE0, 0x08, 3},
+        [REG_INDI_CTRL] = {0xA0, 0xC0, 6},
+        [REG_OPMODE] = {0xA0, 0x03, 0},
+};
+
+static struct lm355x_reg_data lm3556_regs[REG_MAX] = {
+        [REG_FLAG] = {0x0B, 0xFF, 0},
+        [REG_TORCH_CFG] = {0x0A, 0x10, 4},
+        [REG_TORCH_CTRL] = {0x09, 0x70, 4},
+        [REG_STROBE_CFG] = {0x0A, 0x20, 5},
+        [REG_FLASH_CTRL] = {0x09, 0x0F, 0},
+        [REG_INDI_CFG] = {0xFF, 0xFF, 0},
+        [REG_INDI_CTRL] = {0x09, 0x70, 4},
+        [REG_OPMODE] = {0x0A, 0x03, 0},
+};
+
+static char lm355x_name[][I2C_NAME_SIZE] = {
+        [CHIP_LM3554] = LM3554_NAME,
+        [CHIP_LM3556] = LM3556_NAME,
+};
+
+/* chip initialize */
+static int __devinit lm355x_chip_init(struct lm355x_chip_data *chip)
+{
+        int ret;
+        unsigned int reg_val;
+        struct lm355x_platform_data *pdata = chip->pdata;
+
+        /* input and output pins configuration */
+        switch (chip->type) {
+        case CHIP_LM3554:
+                reg_val = pdata->pin_tx2 | pdata->ntc_pin;
+                ret = regmap_update_bits(chip->regmap, 0xE0, 0x28, reg_val);
+                if (ret < 0)
+                        goto out;
+                reg_val = pdata->pass_mode;
+                ret = regmap_update_bits(chip->regmap, 0xA0, 0x04, reg_val);
+                if (ret < 0)
+                        goto out;
+                break;
+
+        case CHIP_LM3556:
+                reg_val = pdata->pin_tx2 | pdata->ntc_pin | pdata->pass_mode;
+                ret = regmap_update_bits(chip->regmap, 0x0A, 0xC4, reg_val);
+                if (ret < 0)
+                        goto out;
+                break;
+        default:
+                return -ENODATA;
+        }
+
+        return ret;
+out:
+        dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+        return ret;
+}
+
+/* chip control */
+static void lm355x_control(struct lm355x_chip_data *chip,
+                           u8 brightness, enum lm355x_mode opmode)
+{
+        int ret;
+        unsigned int reg_val;
+        struct lm355x_platform_data *pdata = chip->pdata;
+        struct lm355x_reg_data *preg = chip->regs;
+
+        ret = regmap_read(chip->regmap, preg[REG_FLAG].regno, &chip->last_flag);
+        if (ret < 0)
+                goto out;
+        if (chip->last_flag & preg[REG_FLAG].mask)
+                dev_info(chip->dev, "%s Last FLAG is 0x%x\n",
+                         lm355x_name[chip->type],
+                         chip->last_flag & preg[REG_FLAG].mask);
+        /* brightness 0 means shutdown */
+        if (!brightness)
+                opmode = MODE_SHDN;
+
+        switch (opmode) {
+        case MODE_TORCH:
+                ret =
+                    regmap_update_bits(chip->regmap, preg[REG_TORCH_CTRL].regno,
+                                       preg[REG_TORCH_CTRL].mask,
+                                       (brightness - 1)
+                                       << preg[REG_TORCH_CTRL].shift);
+                if (ret < 0)
+                        goto out;
+
+                if (pdata->pin_tx1 != LM355x_PIN_TORCH_DISABLE) {
+                        ret =
+                            regmap_update_bits(chip->regmap,
+                                               preg[REG_TORCH_CFG].regno,
+                                               preg[REG_TORCH_CFG].mask,
+                                               0x01 <<
+                                               preg[REG_TORCH_CFG].shift);
+                        if (ret < 0)
+                                goto out;
+                        opmode = MODE_SHDN;
+                        dev_info(chip->dev,
+                                 "torch brt is set - ext. torch pin mode\n");
+                }
+                break;
+
+        case MODE_FLASH:
+
+                ret =
+                    regmap_update_bits(chip->regmap, preg[REG_FLASH_CTRL].regno,
+                                       preg[REG_FLASH_CTRL].mask,
+                                       (brightness - 1)
+                                       << preg[REG_FLASH_CTRL].shift);
+                if (ret < 0)
+                        goto out;
+
+                if (pdata->pin_strobe != LM355x_PIN_STROBE_DISABLE) {
+                        if (chip->type == CHIP_LM3554)
+                                reg_val = 0x00;
+                        else
+                                reg_val = 0x01;
+                        ret =
+                            regmap_update_bits(chip->regmap,
+                                               preg[REG_STROBE_CFG].regno,
+                                               preg[REG_STROBE_CFG].mask,
+                                               reg_val <<
+                                               preg[REG_STROBE_CFG].shift);
+                        if (ret < 0)
+                                goto out;
+                        opmode = MODE_SHDN;
+                        dev_info(chip->dev,
+                                 "flash brt is set - ext. strobe pin mode\n");
+                }
+                break;
+
+        case MODE_INDIC:
+                ret =
+                    regmap_update_bits(chip->regmap, preg[REG_INDI_CTRL].regno,
+                                       preg[REG_INDI_CTRL].mask,
+                                       (brightness - 1)
+                                       << preg[REG_INDI_CTRL].shift);
+                if (ret < 0)
+                        goto out;
+
+                if (pdata->pin_tx2 != LM355x_PIN_TX_DISABLE) {
+                        ret =
+                            regmap_update_bits(chip->regmap,
+                                               preg[REG_INDI_CFG].regno,
+                                               preg[REG_INDI_CFG].mask,
+                                               0x01 <<
+                                               preg[REG_INDI_CFG].shift);
+                        if (ret < 0)
+                                goto out;
+                        opmode = MODE_SHDN;
+                }
+                break;
+        case MODE_SHDN:
+                break;
+        default:
+                return;
+        }
+        /* operation mode control */
+        ret = regmap_update_bits(chip->regmap, preg[REG_OPMODE].regno,
+                                 preg[REG_OPMODE].mask,
+                                 opmode << preg[REG_OPMODE].shift);
+        if (ret < 0)
+                goto out;
+        return;
+out:
+        dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+        return;
+}
+
+/* torch */
+static void lm355x_deferred_torch_brightness_set(struct work_struct *work)
+{
+        struct lm355x_chip_data *chip =
+            container_of(work, struct lm355x_chip_data, work_torch);
+
+        mutex_lock(&chip->lock);
+        lm355x_control(chip, chip->br_torch, MODE_TORCH);
+        mutex_unlock(&chip->lock);
+}
+
+static void lm355x_torch_brightness_set(struct led_classdev *cdev,
+                                        enum led_brightness brightness)
+{
+        struct lm355x_chip_data *chip =
+            container_of(cdev, struct lm355x_chip_data, cdev_torch);
+
+        chip->br_torch = brightness;
+        schedule_work(&chip->work_torch);
+}
+
+/* flash */
+static void lm355x_deferred_strobe_brightness_set(struct work_struct *work)
+{
+        struct lm355x_chip_data *chip =
+            container_of(work, struct lm355x_chip_data, work_flash);
+
+        mutex_lock(&chip->lock);
+        lm355x_control(chip, chip->br_flash, MODE_FLASH);
+        mutex_unlock(&chip->lock);
+}
+
+static void lm355x_strobe_brightness_set(struct led_classdev *cdev,
+                                         enum led_brightness brightness)
+{
+        struct lm355x_chip_data *chip =
+            container_of(cdev, struct lm355x_chip_data, cdev_flash);
+
+        chip->br_flash = brightness;
+        schedule_work(&chip->work_flash);
+}
+
+/* indicator */
+static void lm355x_deferred_indicator_brightness_set(struct work_struct *work)
+{
+        struct lm355x_chip_data *chip =
+            container_of(work, struct lm355x_chip_data, work_indicator);
+
+        mutex_lock(&chip->lock);
+        lm355x_control(chip, chip->br_indicator, MODE_INDIC);
+        mutex_unlock(&chip->lock);
+}
+
+static void lm355x_indicator_brightness_set(struct led_classdev *cdev,
+                                            enum led_brightness brightness)
+{
+        struct lm355x_chip_data *chip =
+            container_of(cdev, struct lm355x_chip_data, cdev_indicator);
+
+        chip->br_indicator = brightness;
+        schedule_work(&chip->work_indicator);
+}
+
+/* indicator pattern only for lm3556*/
+static ssize_t lm3556_indicator_pattern_store(struct device *dev,
+                                              struct device_attribute *devAttr,
+                                              const char *buf, size_t size)
+{
+        ssize_t ret;
+        struct led_classdev *led_cdev = dev_get_drvdata(dev);
+        struct lm355x_chip_data *chip =
+            container_of(led_cdev, struct lm355x_chip_data, cdev_indicator);
+        unsigned int state;
+
+        ret = kstrtouint(buf, 10, &state);
+        if (ret)
+                goto out;
+        if (state > INDIC_PATTERN_SIZE - 1)
+                state = INDIC_PATTERN_SIZE - 1;
+
+        ret = regmap_write(chip->regmap, 0x04,
+                           indicator_pattern[state].blinking);
+        if (ret < 0)
+                goto out;
+
+        ret = regmap_write(chip->regmap, 0x05,
+                           indicator_pattern[state].period_cnt);
+        if (ret < 0)
+                goto out;
+
+        return size;
+out:
+        dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+        return size;
+}
+
+static DEVICE_ATTR(pattern, 0666, NULL, lm3556_indicator_pattern_store);
+
+static const struct regmap_config lm355x_regmap = {
+        .reg_bits = 8,
+        .val_bits = 8,
+        .max_register = 0xFF,
+};
+
+/* module initialize */
+static int __devinit lm355x_probe(struct i2c_client *client,
+                                  const struct i2c_device_id *id)
+{
+        struct lm355x_platform_data *pdata = client->dev.platform_data;
+        struct lm355x_chip_data *chip;
+
+        int err;
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                dev_err(&client->dev, "i2c functionality check fail.\n");
+                return -EOPNOTSUPP;
+        }
+
+        if (pdata == NULL) {
+                dev_err(&client->dev, "needs Platform Data.\n");
+                return -ENODATA;
+        }
+
+        chip = devm_kzalloc(&client->dev,
+                            sizeof(struct lm355x_chip_data), GFP_KERNEL);
+        if (!chip)
+                return -ENOMEM;
+
+        chip->dev = &client->dev;
+        chip->type = id->driver_data;
+        switch (id->driver_data) {
+        case CHIP_LM3554:
+                chip->regs = lm3554_regs;
+                break;
+        case CHIP_LM3556:
+                chip->regs = lm3556_regs;
+                break;
+        default:
+                return -ENOSYS;
+        }
+        chip->pdata = pdata;
+
+        chip->regmap = devm_regmap_init_i2c(client, &lm355x_regmap);
+        if (IS_ERR(chip->regmap)) {
+                err = PTR_ERR(chip->regmap);
+                dev_err(&client->dev,
+                        "Failed to allocate register map: %d\n", err);
+                return err;
+        }
+
+        mutex_init(&chip->lock);
+        i2c_set_clientdata(client, chip);
+
+        err = lm355x_chip_init(chip);
+        if (err < 0)
+                goto err_out;
+
+        /* flash */
+        INIT_WORK(&chip->work_flash, lm355x_deferred_strobe_brightness_set);
+        chip->cdev_flash.name = "flash";
+        chip->cdev_flash.max_brightness = 16;
+        chip->cdev_flash.brightness_set = lm355x_strobe_brightness_set;
+        err = led_classdev_register((struct device *)
+                                    &client->dev, &chip->cdev_flash);
+        if (err < 0)
+                goto err_out;
+        /* torch */
+        INIT_WORK(&chip->work_torch, lm355x_deferred_torch_brightness_set);
+        chip->cdev_torch.name = "torch";
+        chip->cdev_torch.max_brightness = 8;
+        chip->cdev_torch.brightness_set = lm355x_torch_brightness_set;
+        err = led_classdev_register((struct device *)
+                                    &client->dev, &chip->cdev_torch);
+        if (err < 0)
+                goto err_create_torch_file;
+        /* indicator */
+        INIT_WORK(&chip->work_indicator,
+                  lm355x_deferred_indicator_brightness_set);
+        chip->cdev_indicator.name = "indicator";
+        if (id->driver_data == CHIP_LM3554)
+                chip->cdev_indicator.max_brightness = 4;
+        else
+                chip->cdev_indicator.max_brightness = 8;
+        chip->cdev_indicator.brightness_set = lm355x_indicator_brightness_set;
+        err = led_classdev_register((struct device *)
+                                    &client->dev, &chip->cdev_indicator);
+        if (err < 0)
+                goto err_create_indicator_file;
+        /* indicator pattern control only for LM3554 */
+        if (id->driver_data == CHIP_LM3556) {
+                err =
+                    device_create_file(chip->cdev_indicator.dev,
+                                       &dev_attr_pattern);
+                if (err < 0)
+                        goto err_create_pattern_file;
+        }
+
+        dev_info(&client->dev, "%s is initialized\n",
+                 lm355x_name[id->driver_data]);
+        return 0;
+
+err_create_pattern_file:
+        led_classdev_unregister(&chip->cdev_indicator);
+err_create_indicator_file:
+        led_classdev_unregister(&chip->cdev_torch);
+err_create_torch_file:
+        led_classdev_unregister(&chip->cdev_flash);
+err_out:
+        return err;
+}
+
+static int __devexit lm355x_remove(struct i2c_client *client)
+{
+        struct lm355x_chip_data *chip = i2c_get_clientdata(client);
+        struct lm355x_reg_data *preg = chip->regs;
+
+        regmap_write(chip->regmap, preg[REG_OPMODE].regno, 0);
+        if (chip->type == CHIP_LM3556)
+                device_remove_file(chip->cdev_indicator.dev, &dev_attr_pattern);
+        led_classdev_unregister(&chip->cdev_indicator);
+        flush_work(&chip->work_indicator);
+        led_classdev_unregister(&chip->cdev_torch);
+        flush_work(&chip->work_torch);
+        led_classdev_unregister(&chip->cdev_flash);
+        flush_work(&chip->work_flash);
+        dev_info(&client->dev, "%s is removed\n", lm355x_name[chip->type]);
+
+        return 0;
+}
+
+static const struct i2c_device_id lm355x_id[] = {
+        {LM3554_NAME, CHIP_LM3554},
+        {LM3556_NAME, CHIP_LM3556},
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm355x_id);
+
+static struct i2c_driver lm355x_i2c_driver = {
+        .driver = {
+                   .name = LM355x_NAME,
+                   .owner = THIS_MODULE,
+                   .pm = NULL,
+                   },
+        .probe = lm355x_probe,
+        .remove = __devexit_p(lm355x_remove),
+        .id_table = lm355x_id,
+};
+
+module_i2c_driver(lm355x_i2c_driver);
+
+MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM355x");
+MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
+MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lm3642.c b/drivers/leds/leds-lm3642.c
new file mode 100644
index 000000000000..3285006e9888
--- /dev/null
+++ b/drivers/leds/leds-lm3642.c
@@ -0,0 +1,462 @@
+/*
+* Simple driver for Texas Instruments LM3642 LED Flash driver chip
+* Copyright (C) 2012 Texas Instruments
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+*/
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/regmap.h>
+#include <linux/workqueue.h>
+#include <linux/platform_data/leds-lm3642.h>
+
+#define REG_FILT_TIME                   (0x0)
+#define REG_IVFM_MODE                   (0x1)
+#define REG_TORCH_TIME                  (0x6)
+#define REG_FLASH                       (0x8)
+#define REG_I_CTRL                      (0x9)
+#define REG_ENABLE                      (0xA)
+#define REG_FLAG                        (0xB)
+#define REG_MAX                         (0xB)
+
+#define UVLO_EN_SHIFT                   (7)
+#define IVM_D_TH_SHIFT                  (2)
+#define TORCH_RAMP_UP_TIME_SHIFT        (3)
+#define TORCH_RAMP_DN_TIME_SHIFT        (0)
+#define INDUCTOR_I_LIMIT_SHIFT          (6)
+#define FLASH_RAMP_TIME_SHIFT           (3)
+#define FLASH_TOUT_TIME_SHIFT           (0)
+#define TORCH_I_SHIFT                   (4)
+#define FLASH_I_SHIFT                   (0)
+#define IVFM_SHIFT                      (7)
+#define TX_PIN_EN_SHIFT                 (6)
+#define STROBE_PIN_EN_SHIFT             (5)
+#define TORCH_PIN_EN_SHIFT              (4)
+#define MODE_BITS_SHIFT                 (0)
+
+#define UVLO_EN_MASK                    (0x1)
+#define IVM_D_TH_MASK                   (0x7)
+#define TORCH_RAMP_UP_TIME_MASK         (0x7)
+#define TORCH_RAMP_DN_TIME_MASK         (0x7)
+#define INDUCTOR_I_LIMIT_MASK           (0x1)
+#define FLASH_RAMP_TIME_MASK            (0x7)
+#define FLASH_TOUT_TIME_MASK            (0x7)
+#define TORCH_I_MASK                    (0x7)
+#define FLASH_I_MASK                    (0xF)
+#define IVFM_MASK                       (0x1)
+#define TX_PIN_EN_MASK                  (0x1)
+#define STROBE_PIN_EN_MASK              (0x1)
+#define TORCH_PIN_EN_MASK               (0x1)
+#define MODE_BITS_MASK                  (0x73)
+#define EX_PIN_CONTROL_MASK             (0x71)
+#define EX_PIN_ENABLE_MASK              (0x70)
+
+enum lm3642_mode {
+        MODES_STASNDBY = 0,
+        MODES_INDIC,
+        MODES_TORCH,
+        MODES_FLASH
+};
+
+struct lm3642_chip_data {
+        struct device *dev;
+
+        struct led_classdev cdev_flash;
+        struct led_classdev cdev_torch;
+        struct led_classdev cdev_indicator;
+
+        struct work_struct work_flash;
+        struct work_struct work_torch;
+        struct work_struct work_indicator;
+
+        u8 br_flash;
+        u8 br_torch;
+        u8 br_indicator;
+
+        enum lm3642_torch_pin_enable torch_pin;
+        enum lm3642_strobe_pin_enable strobe_pin;
+        enum lm3642_tx_pin_enable tx_pin;
+
+        struct lm3642_platform_data *pdata;
+        struct regmap *regmap;
+        struct mutex lock;
+
+        unsigned int last_flag;
+};
+
+/* chip initialize */
+static int __devinit lm3642_chip_init(struct lm3642_chip_data *chip)
+{
+        int ret;
+        struct lm3642_platform_data *pdata = chip->pdata;
+
+        /* set enable register */
+        ret = regmap_update_bits(chip->regmap, REG_ENABLE, EX_PIN_ENABLE_MASK,
+                                 pdata->tx_pin);
+        if (ret < 0)
+                dev_err(chip->dev, "Failed to update REG_ENABLE Register\n");
+        return ret;
+}
+
+/* chip control */
+static int lm3642_control(struct lm3642_chip_data *chip,
+                          u8 brightness, enum lm3642_mode opmode)
+{
+        int ret;
+
+        ret = regmap_read(chip->regmap, REG_FLAG, &chip->last_flag);
+        if (ret < 0) {
+                dev_err(chip->dev, "Failed to read REG_FLAG Register\n");
+                goto out;
+        }
+
+        if (chip->last_flag)
+                dev_info(chip->dev, "Last FLAG is 0x%x\n", chip->last_flag);
+
+        /* brightness 0 means off-state */
+        if (!brightness)
+                opmode = MODES_STASNDBY;
+
+        switch (opmode) {
+        case MODES_TORCH:
+                ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+                                         TORCH_I_MASK << TORCH_I_SHIFT,
+                                         (brightness - 1) << TORCH_I_SHIFT);
+
+                if (chip->torch_pin)
+                        opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
+                break;
+
+        case MODES_FLASH:
+                ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+                                         FLASH_I_MASK << FLASH_I_SHIFT,
+                                         (brightness - 1) << FLASH_I_SHIFT);
+
+                if (chip->strobe_pin)
+                        opmode |= (STROBE_PIN_EN_MASK << STROBE_PIN_EN_SHIFT);
+                break;
+
+        case MODES_INDIC:
+                ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+                                         TORCH_I_MASK << TORCH_I_SHIFT,
+                                         (brightness - 1) << TORCH_I_SHIFT);
+                break;
+
+        case MODES_STASNDBY:
+
+                break;
+
+        default:
+                return ret;
+        }
+        if (ret < 0) {
+                dev_err(chip->dev, "Failed to write REG_I_CTRL Register\n");
+                goto out;
+        }
+
+        if (chip->tx_pin)
+                opmode |= (TX_PIN_EN_MASK << TX_PIN_EN_SHIFT);
+
+        ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+                                 MODE_BITS_MASK << MODE_BITS_SHIFT,
+                                 opmode << MODE_BITS_SHIFT);
+out:
+        return ret;
+}
+
+/* torch */
+
+/* torch pin config for lm3642*/
+static ssize_t lm3642_torch_pin_store(struct device *dev,
+                                      struct device_attribute *devAttr,
+                                      const char *buf, size_t size)
+{
+        ssize_t ret;
+        struct led_classdev *led_cdev = dev_get_drvdata(dev);
+        struct lm3642_chip_data *chip =
+            container_of(led_cdev, struct lm3642_chip_data, cdev_indicator);
+        unsigned int state;
+
+        ret = kstrtouint(buf, 10, &state);
+        if (ret)
+                goto out_strtoint;
+        if (state != 0)
+                state = 0x01 << TORCH_PIN_EN_SHIFT;
+
+        chip->torch_pin = state;
+        ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+                                 TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT,
+                                 state);
+        if (ret < 0)
+                goto out;
+
+        return size;
+out:
+        dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+        return size;
+out_strtoint:
+        dev_err(chip->dev, "%s: fail to change str to int\n", __func__);
+        return size;
+}
+
+static DEVICE_ATTR(torch_pin, 0666, NULL, lm3642_torch_pin_store);
+
+static void lm3642_deferred_torch_brightness_set(struct work_struct *work)
+{
+        struct lm3642_chip_data *chip =
+            container_of(work, struct lm3642_chip_data, work_torch);
+
+        mutex_lock(&chip->lock);
+        lm3642_control(chip, chip->br_torch, MODES_TORCH);
+        mutex_unlock(&chip->lock);
+}
+
+static void lm3642_torch_brightness_set(struct led_classdev *cdev,
+                                        enum led_brightness brightness)
+{
+        struct lm3642_chip_data *chip =
+            container_of(cdev, struct lm3642_chip_data, cdev_torch);
+
+        chip->br_torch = brightness;
+        schedule_work(&chip->work_torch);
+}
+
+/* flash */
+
+/* strobe pin config for lm3642*/
+static ssize_t lm3642_strobe_pin_store(struct device *dev,
+                                       struct device_attribute *devAttr,
+                                       const char *buf, size_t size)
+{
+        ssize_t ret;
+        struct led_classdev *led_cdev = dev_get_drvdata(dev);
+        struct lm3642_chip_data *chip =
+            container_of(led_cdev, struct lm3642_chip_data, cdev_indicator);
+        unsigned int state;
+
+        ret = kstrtouint(buf, 10, &state);
+        if (ret)
+                goto out_strtoint;
+        if (state != 0)
+                state = 0x01 << STROBE_PIN_EN_SHIFT;
+
+        chip->strobe_pin = state;
+        ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+                                 STROBE_PIN_EN_MASK << STROBE_PIN_EN_SHIFT,
+                                 state);
+        if (ret < 0)
+                goto out;
+
+        return size;
+out:
+        dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+        return size;
+out_strtoint:
+        dev_err(chip->dev, "%s: fail to change str to int\n", __func__);
+        return size;
+}
+
+static DEVICE_ATTR(strobe_pin, 0666, NULL, lm3642_strobe_pin_store);
+
+static void lm3642_deferred_strobe_brightness_set(struct work_struct *work)
+{
+        struct lm3642_chip_data *chip =
+            container_of(work, struct lm3642_chip_data, work_flash);
+
+        mutex_lock(&chip->lock);
+        lm3642_control(chip, chip->br_flash, MODES_FLASH);
+        mutex_unlock(&chip->lock);
+}
+
+static void lm3642_strobe_brightness_set(struct led_classdev *cdev,
+                                         enum led_brightness brightness)
+{
+        struct lm3642_chip_data *chip =
+            container_of(cdev, struct lm3642_chip_data, cdev_flash);
+
+        chip->br_flash = brightness;
+        schedule_work(&chip->work_flash);
+}
+
+/* indicator */
+static void lm3642_deferred_indicator_brightness_set(struct work_struct *work)
+{
+        struct lm3642_chip_data *chip =
+            container_of(work, struct lm3642_chip_data, work_indicator);
+
+        mutex_lock(&chip->lock);
+        lm3642_control(chip, chip->br_indicator, MODES_INDIC);
+        mutex_unlock(&chip->lock);
+}
+
+static void lm3642_indicator_brightness_set(struct led_classdev *cdev,
+                                            enum led_brightness brightness)
+{
+        struct lm3642_chip_data *chip =
+            container_of(cdev, struct lm3642_chip_data, cdev_indicator);
+
+        chip->br_indicator = brightness;
+        schedule_work(&chip->work_indicator);
+}
+
+static const struct regmap_config lm3642_regmap = {
+        .reg_bits = 8,
+        .val_bits = 8,
+        .max_register = REG_MAX,
+};
+
+static int __devinit lm3642_probe(struct i2c_client *client,
+                                  const struct i2c_device_id *id)
+{
+        struct lm3642_platform_data *pdata = client->dev.platform_data;
+        struct lm3642_chip_data *chip;
+
+        int err;
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                dev_err(&client->dev, "i2c functionality check fail.\n");
+                return -EOPNOTSUPP;
+        }
+
+        if (pdata == NULL) {
+                dev_err(&client->dev, "needs Platform Data.\n");
+                return -ENODATA;
+        }
+
+        chip = devm_kzalloc(&client->dev,
+                            sizeof(struct lm3642_chip_data), GFP_KERNEL);
+        if (!chip)
+                return -ENOMEM;
+
+        chip->dev = &client->dev;
+        chip->pdata = pdata;
+
+        chip->tx_pin = pdata->tx_pin;
+        chip->torch_pin = pdata->torch_pin;
+        chip->strobe_pin = pdata->strobe_pin;
+
+        chip->regmap = devm_regmap_init_i2c(client, &lm3642_regmap);
+        if (IS_ERR(chip->regmap)) {
+                err = PTR_ERR(chip->regmap);
+                dev_err(&client->dev, "Failed to allocate register map: %d\n",
+                        err);
+                return err;
+        }
+
+        mutex_init(&chip->lock);
+        i2c_set_clientdata(client, chip);
+
+        err = lm3642_chip_init(chip);
+        if (err < 0)
+                goto err_out;
+
+        /* flash */
+        INIT_WORK(&chip->work_flash, lm3642_deferred_strobe_brightness_set);
+        chip->cdev_flash.name = "flash";
+        chip->cdev_flash.max_brightness = 16;
+        chip->cdev_flash.brightness_set = lm3642_strobe_brightness_set;
+        err = led_classdev_register((struct device *)
+                                    &client->dev, &chip->cdev_flash);
+        if (err < 0) {
+                dev_err(chip->dev, "failed to register flash\n");
+                goto err_out;
+        }
+        err = device_create_file(chip->cdev_flash.dev, &dev_attr_strobe_pin);
+        if (err < 0) {
+                dev_err(chip->dev, "failed to create strobe-pin file\n");
+                goto err_create_flash_pin_file;
+        }
+
+        /* torch */
+        INIT_WORK(&chip->work_torch, lm3642_deferred_torch_brightness_set);
+        chip->cdev_torch.name = "torch";
+        chip->cdev_torch.max_brightness = 8;
+        chip->cdev_torch.brightness_set = lm3642_torch_brightness_set;
+        err = led_classdev_register((struct device *)
+                                    &client->dev, &chip->cdev_torch);
+        if (err < 0) {
+                dev_err(chip->dev, "failed to register torch\n");
+                goto err_create_torch_file;
+        }
+        err = device_create_file(chip->cdev_torch.dev, &dev_attr_torch_pin);
+        if (err < 0) {
+                dev_err(chip->dev, "failed to create torch-pin file\n");
+                goto err_create_torch_pin_file;
+        }
+
+        /* indicator */
+        INIT_WORK(&chip->work_indicator,
+                  lm3642_deferred_indicator_brightness_set);
+        chip->cdev_indicator.name = "indicator";
+        chip->cdev_indicator.max_brightness = 8;
+        chip->cdev_indicator.brightness_set = lm3642_indicator_brightness_set;
+        err = led_classdev_register((struct device *)
+                                    &client->dev, &chip->cdev_indicator);
+        if (err < 0) {
+                dev_err(chip->dev, "failed to register indicator\n");
+                goto err_create_indicator_file;
+        }
+
+        dev_info(&client->dev, "LM3642 is initialized\n");
+        return 0;
+
+err_create_indicator_file:
+        device_remove_file(chip->cdev_torch.dev, &dev_attr_torch_pin);
+err_create_torch_pin_file:
+        led_classdev_unregister(&chip->cdev_torch);
+err_create_torch_file:
+        device_remove_file(chip->cdev_flash.dev, &dev_attr_strobe_pin);
+err_create_flash_pin_file:
+        led_classdev_unregister(&chip->cdev_flash);
+err_out:
+        return err;
+}
+
+static int __devexit lm3642_remove(struct i2c_client *client)
+{
+        struct lm3642_chip_data *chip = i2c_get_clientdata(client);
+
+        led_classdev_unregister(&chip->cdev_indicator);
+        flush_work(&chip->work_indicator);
+        device_remove_file(chip->cdev_torch.dev, &dev_attr_torch_pin);
+        led_classdev_unregister(&chip->cdev_torch);
+        flush_work(&chip->work_torch);
+        device_remove_file(chip->cdev_flash.dev, &dev_attr_strobe_pin);
+        led_classdev_unregister(&chip->cdev_flash);
+        flush_work(&chip->work_flash);
+        regmap_write(chip->regmap, REG_ENABLE, 0);
+        return 0;
+}
+
+static const struct i2c_device_id lm3642_id[] = {
+        {LM3642_NAME, 0},
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm3642_id);
+
+static struct i2c_driver lm3642_i2c_driver = {
+        .driver = {
+                   .name = LM3642_NAME,
+                   .owner = THIS_MODULE,
+                   .pm = NULL,
+                   },
+        .probe = lm3642_probe,
+        .remove = __devexit_p(lm3642_remove),
+        .id_table = lm3642_id,
+};
+
+module_i2c_driver(lm3642_i2c_driver);
+
+MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM3642");
+MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
+MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index fbc12acada95..97994ffdc014 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -104,6 +104,11 @@
 #define LED_ACTIVE(mux, led)            (!!(mux & (0x0001 << led)))
 #define SHIFT_MASK(id)                  (((id) - 1) * 2)
 
+enum lp5523_chip_id {
+        LP5523,
+        LP55231,
+};
+
 struct lp5523_engine {
         int             id;
         u8              mode;
@@ -150,7 +155,7 @@ static inline struct lp5523_chip *led_to_lp5523(struct lp5523_led *led)
                             leds[led->id]);
 }
 
-static int lp5523_set_mode(struct lp5523_engine *engine, u8 mode);
+static void lp5523_set_mode(struct lp5523_engine *engine, u8 mode);
 static int lp5523_set_engine_mode(struct lp5523_engine *engine, u8 mode);
 static int lp5523_load_program(struct lp5523_engine *engine, const u8 *pattern);
 
@@ -177,7 +182,7 @@ static int lp5523_detect(struct i2c_client *client)
         int ret;
         u8 buf;
 
-        ret = lp5523_write(client, LP5523_REG_ENABLE, 0x40);
+        ret = lp5523_write(client, LP5523_REG_ENABLE, LP5523_ENABLE);
         if (ret)
                 return ret;
         ret = lp5523_read(client, LP5523_REG_ENABLE, &buf);
@@ -338,7 +343,8 @@ static int lp5523_mux_parse(const char *buf, u16 *mux, size_t len)
 {
         int i;
         u16 tmp_mux = 0;
-        len = len < LP5523_LEDS ? len : LP5523_LEDS;
+
+        len = min_t(int, len, LP5523_LEDS);
         for (i = 0; i < len; i++) {
                 switch (buf[i]) {
                 case '1':
@@ -546,6 +552,9 @@ static int lp5523_do_store_load(struct lp5523_engine *engine,
         unsigned cmd;
         u8 pattern[LP5523_PROGRAM_LENGTH] = {0};
 
+        if (engine->mode != LP5523_CMD_LOAD)
+                return -EINVAL;
+
         while ((offset < len - 1) && (i < LP5523_PROGRAM_LENGTH)) {
                 /* separate sscanfs because length is working only for %s */
                 ret = sscanf(buf + offset, "%2s%n ", c, &nrchars);
@@ -563,12 +572,7 @@ static int lp5523_do_store_load(struct lp5523_engine *engine,
                 goto fail;
 
         mutex_lock(&chip->lock);
-
-        if (engine->mode == LP5523_CMD_LOAD)
-                ret = lp5523_load_program(engine, pattern);
-        else
-                ret = -EINVAL;
-
+        ret = lp5523_load_program(engine, pattern);
         mutex_unlock(&chip->lock);
 
         if (ret) {
@@ -755,6 +759,7 @@ static struct attribute *lp5523_attributes[] = {
         &dev_attr_engine2_leds.attr,
         &dev_attr_engine3_load.attr,
         &dev_attr_engine3_leds.attr,
+        NULL,
 };
 
 static const struct attribute_group lp5523_group = {
@@ -789,26 +794,28 @@ static void lp5523_unregister_sysfs(struct i2c_client *client)
 /*--------------------------------------------------------------*/
 /*                      Set chip operating mode                 */
 /*--------------------------------------------------------------*/
-static int lp5523_set_mode(struct lp5523_engine *engine, u8 mode)
+static void lp5523_set_mode(struct lp5523_engine *engine, u8 mode)
 {
-        int ret = 0;
-
         /* if in that mode already do nothing, except for run */
         if (mode == engine->mode && mode != LP5523_CMD_RUN)
-                return 0;
+                return;
 
-        if (mode == LP5523_CMD_RUN) {
-                ret = lp5523_run_program(engine);
-        } else if (mode == LP5523_CMD_LOAD) {
+        switch (mode) {
+        case LP5523_CMD_RUN:
+                lp5523_run_program(engine);
+                break;
+        case LP5523_CMD_LOAD:
                 lp5523_set_engine_mode(engine, LP5523_CMD_DISABLED);
                 lp5523_set_engine_mode(engine, LP5523_CMD_LOAD);
-        } else if (mode == LP5523_CMD_DISABLED) {
+                break;
+        case LP5523_CMD_DISABLED:
                 lp5523_set_engine_mode(engine, LP5523_CMD_DISABLED);
+                break;
+        default:
+                return;
         }
 
         engine->mode = mode;
-
-        return ret;
 }
 
 /*--------------------------------------------------------------*/
@@ -827,7 +834,8 @@ static int __init lp5523_init_engine(struct lp5523_engine *engine, int id)
 }
 
 static int __devinit lp5523_init_led(struct lp5523_led *led, struct device *dev,
-                           int chan, struct lp5523_platform_data *pdata)
+                           int chan, struct lp5523_platform_data *pdata,
+                           const char *chip_name)
 {
         char name[32];
         int res;
@@ -846,10 +854,14 @@ static int __devinit lp5523_init_led(struct lp5523_led *led, struct device *dev,
                         return -EINVAL;
                 }
 
-                snprintf(name, sizeof(name), "%s:channel%d",
-                        pdata->label ?: "lp5523", chan);
+                if (pdata->led_config[chan].name) {
+                        led->cdev.name = pdata->led_config[chan].name;
+                } else {
+                        snprintf(name, sizeof(name), "%s:channel%d",
+                                pdata->label ? : chip_name, chan);
+                        led->cdev.name = name;
+                }
 
-                led->cdev.name = name;
                 led->cdev.brightness_set = lp5523_set_brightness;
                 res = led_classdev_register(dev, &led->cdev);
                 if (res < 0) {
@@ -917,7 +929,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
         if (ret)
                 goto fail1;
 
-        dev_info(&client->dev, "LP5523 Programmable led chip found\n");
+        dev_info(&client->dev, "%s Programmable led chip found\n", id->name);
 
         /* Initialize engines */
         for (i = 0; i < ARRAY_SIZE(chip->engines); i++) {
@@ -945,7 +957,8 @@ static int __devinit lp5523_probe(struct i2c_client *client,
                 INIT_WORK(&chip->leds[led].brightness_work,
                         lp5523_led_brightness_work);
 
-                ret = lp5523_init_led(&chip->leds[led], &client->dev, i, pdata);
+                ret = lp5523_init_led(&chip->leds[led], &client->dev, i, pdata,
+                                id->name);
                 if (ret) {
                         dev_err(&client->dev, "error initializing leds\n");
                         goto fail2;
@@ -970,7 +983,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 fail2:
         for (i = 0; i < chip->num_leds; i++) {
                 led_classdev_unregister(&chip->leds[i].cdev);
-                cancel_work_sync(&chip->leds[i].brightness_work);
+                flush_work(&chip->leds[i].brightness_work);
         }
 fail1:
         if (pdata->enable)
@@ -985,11 +998,14 @@ static int lp5523_remove(struct i2c_client *client)
         struct lp5523_chip *chip = i2c_get_clientdata(client);
         int i;
 
+        /* Disable engine mode */
+        lp5523_write(client, LP5523_REG_OP_MODE, LP5523_CMD_DISABLED);
+
         lp5523_unregister_sysfs(client);
 
         for (i = 0; i < chip->num_leds; i++) {
                 led_classdev_unregister(&chip->leds[i].cdev);
-                cancel_work_sync(&chip->leds[i].brightness_work);
+                flush_work(&chip->leds[i].brightness_work);
         }
 
         if (chip->pdata->enable)
@@ -1000,7 +1016,8 @@ static int lp5523_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id lp5523_id[] = {
-        { "lp5523", 0 },
+        { "lp5523",  LP5523 },
+        { "lp55231", LP55231 },
         { }
 };
 
@@ -1008,7 +1025,7 @@ MODULE_DEVICE_TABLE(i2c, lp5523_id);
 
 static struct i2c_driver lp5523_driver = {
         .driver = {
-                .name   = "lp5523",
+                .name   = "lp5523x",
         },
         .probe          = lp5523_probe,
         .remove         = lp5523_remove,
diff --git a/drivers/leds/leds-pca9633.c b/drivers/leds/leds-pca9633.c
index edcd706c5631..2f2f9c43535d 100644
--- a/drivers/leds/leds-pca9633.c
+++ b/drivers/leds/leds-pca9633.c
@@ -22,6 +22,7 @@
 #include <linux/i2c.h>
 #include <linux/workqueue.h>
 #include <linux/slab.h>
+#include <linux/platform_data/leds-pca9633.h>
 
 /* LED select registers determine the source that drives LED outputs */
 #define PCA9633_LED_OFF         0x0     /* LED driver off */
@@ -96,13 +97,13 @@ static int __devinit pca9633_probe(struct i2c_client *client,
                                         const struct i2c_device_id *id)
 {
         struct pca9633_led *pca9633;
-        struct led_platform_data *pdata;
+        struct pca9633_platform_data *pdata;
         int i, err;
 
         pdata = client->dev.platform_data;
 
         if (pdata) {
-                if (pdata->num_leds <= 0 || pdata->num_leds > 4) {
+                if (pdata->leds.num_leds <= 0 || pdata->leds.num_leds > 4) {
                         dev_err(&client->dev, "board info must claim at most 4 LEDs");
                         return -EINVAL;
                 }
@@ -119,14 +120,14 @@ static int __devinit pca9633_probe(struct i2c_client *client,
                 pca9633[i].led_num = i;
 
                 /* Platform data can specify LED names and default triggers */
-                if (pdata && i < pdata->num_leds) {
-                        if (pdata->leds[i].name)
+                if (pdata && i < pdata->leds.num_leds) {
+                        if (pdata->leds.leds[i].name)
                                 snprintf(pca9633[i].name,
                                          sizeof(pca9633[i].name), "pca9633:%s",
-                                         pdata->leds[i].name);
-                        if (pdata->leds[i].default_trigger)
+                                         pdata->leds.leds[i].name);
+                        if (pdata->leds.leds[i].default_trigger)
                                 pca9633[i].led_cdev.default_trigger =
-                                        pdata->leds[i].default_trigger;
+                                        pdata->leds.leds[i].default_trigger;
                 } else {
                         snprintf(pca9633[i].name, sizeof(pca9633[i].name),
                                  "pca9633:%d", i);
@@ -145,6 +146,10 @@ static int __devinit pca9633_probe(struct i2c_client *client,
         /* Disable LED all-call address and set normal mode */
         i2c_smbus_write_byte_data(client, PCA9633_MODE1, 0x00);
 
+        /* Configure output: open-drain or totem pole (push-pull) */
+        if (pdata && pdata->outdrv == PCA9633_OPEN_DRAIN)
+                i2c_smbus_write_byte_data(client, PCA9633_MODE2, 0x01);
+
         /* Turn off LEDs */
         i2c_smbus_write_byte_data(client, PCA9633_LEDOUT, 0x00);
 
diff --git a/drivers/leds/leds-wm8350.c b/drivers/leds/leds-wm8350.c
index 4c62113f7a77..88f23f845595 100644
--- a/drivers/leds/leds-wm8350.c
+++ b/drivers/leds/leds-wm8350.c
@@ -201,7 +201,7 @@ static int wm8350_led_probe(struct platform_device *pdev)
         struct regulator *isink, *dcdc;
         struct wm8350_led *led;
         struct wm8350_led_platform_data *pdata = pdev->dev.platform_data;
-        int ret, i;
+        int i;
 
         if (pdata == NULL) {
                 dev_err(&pdev->dev, "no platform data\n");
@@ -214,24 +214,21 @@ static int wm8350_led_probe(struct platform_device *pdev)
                 return -EINVAL;
         }
 
-        isink = regulator_get(&pdev->dev, "led_isink");
+        isink = devm_regulator_get(&pdev->dev, "led_isink");
         if (IS_ERR(isink)) {
                 printk(KERN_ERR "%s: can't get ISINK\n", __func__);
                 return PTR_ERR(isink);
         }
 
-        dcdc = regulator_get(&pdev->dev, "led_vcc");
+        dcdc = devm_regulator_get(&pdev->dev, "led_vcc");
         if (IS_ERR(dcdc)) {
                 printk(KERN_ERR "%s: can't get DCDC\n", __func__);
-                ret = PTR_ERR(dcdc);
-                goto err_isink;
+                return PTR_ERR(dcdc);
         }
 
         led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
-        if (led == NULL) {
-                ret = -ENOMEM;
-                goto err_dcdc;
-        }
+        if (led == NULL)
+                return -ENOMEM;
 
         led->cdev.brightness_set = wm8350_led_set;
         led->cdev.default_trigger = pdata->default_trigger;
@@ -257,17 +254,7 @@ static int wm8350_led_probe(struct platform_device *pdev)
         led->value = LED_OFF;
         platform_set_drvdata(pdev, led);
 
-        ret = led_classdev_register(&pdev->dev, &led->cdev);
-        if (ret < 0)
-                goto err_dcdc;
-
-        return 0;
-
- err_dcdc:
-        regulator_put(dcdc);
- err_isink:
-        regulator_put(isink);
-        return ret;
+        return led_classdev_register(&pdev->dev, &led->cdev);
 }
 
 static int wm8350_led_remove(struct platform_device *pdev)
@@ -277,8 +264,6 @@ static int wm8350_led_remove(struct platform_device *pdev)
         led_classdev_unregister(&led->cdev);
         flush_work(&led->work);
         wm8350_led_disable(led);
-        regulator_put(led->dcdc);
-        regulator_put(led->isink);
         return 0;
 }
 
diff --git a/drivers/leds/leds.h b/drivers/leds/leds.h
index d02acd496126..4c50365344a9 100644
--- a/drivers/leds/leds.h
+++ b/drivers/leds/leds.h
@@ -32,6 +32,8 @@ static inline int led_get_brightness(struct led_classdev *led_cdev)
         return led_cdev->brightness;
 }
 
+void led_stop_software_blink(struct led_classdev *led_cdev);
+
 extern struct rw_semaphore leds_list_lock;
 extern struct list_head leds_list;
 
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index d949b781f6f8..91a02eeeb319 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -216,6 +216,13 @@ config DM_BUFIO
          as a cache, holding recently-read blocks in memory and performing
          delayed writes.
 
+config DM_BIO_PRISON
+       tristate
+       depends on BLK_DEV_DM && EXPERIMENTAL
+       ---help---
+         Some bio locking schemes used by other device-mapper targets
+         including thin provisioning.
+
 source "drivers/md/persistent-data/Kconfig"
 
 config DM_CRYPT
@@ -247,6 +254,7 @@ config DM_THIN_PROVISIONING
        tristate "Thin provisioning target (EXPERIMENTAL)"
        depends on BLK_DEV_DM && EXPERIMENTAL
        select DM_PERSISTENT_DATA
+       select DM_BIO_PRISON
        ---help---
          Provides thin provisioning and snapshots that share a data store.
 
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index 8b2e0dffe82e..94dce8b49324 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_MD_FAULTY)		+= faulty.o
 obj-$(CONFIG_BLK_DEV_MD)        += md-mod.o
 obj-$(CONFIG_BLK_DEV_DM)        += dm-mod.o
 obj-$(CONFIG_DM_BUFIO)          += dm-bufio.o
+obj-$(CONFIG_DM_BIO_PRISON)     += dm-bio-prison.o
 obj-$(CONFIG_DM_CRYPT)          += dm-crypt.o
 obj-$(CONFIG_DM_DELAY)          += dm-delay.o
 obj-$(CONFIG_DM_FLAKEY)         += dm-flakey.o
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 94e7f6ba2e11..7155945f8eb8 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -163,20 +163,17 @@ static struct md_rdev *next_active_rdev(struct md_rdev *rdev, struct mddev *mdde
          * As devices are only added or removed when raid_disk is < 0 and
          * nr_pending is 0 and In_sync is clear, the entries we return will
          * still be in the same position on the list when we re-enter
-         * list_for_each_continue_rcu.
+         * list_for_each_entry_continue_rcu.
          */
-        struct list_head *pos;
         rcu_read_lock();
         if (rdev == NULL)
                 /* start at the beginning */
-                pos = &mddev->disks;
+                rdev = list_entry_rcu(&mddev->disks, struct md_rdev, same_set);
         else {
                 /* release the previous rdev and start from there. */
                 rdev_dec_pending(rdev, mddev);
-                pos = &rdev->same_set;
         }
-        list_for_each_continue_rcu(pos, &mddev->disks) {
-                rdev = list_entry(pos, struct md_rdev, same_set);
+        list_for_each_entry_continue_rcu(rdev, &mddev->disks, same_set) {
                 if (rdev->raid_disk >= 0 &&
                     !test_bit(Faulty, &rdev->flags)) {
                         /* this is a usable devices */
@@ -473,14 +470,10 @@ static int bitmap_new_disk_sb(struct bitmap *bitmap)
 {
         bitmap_super_t *sb;
         unsigned long chunksize, daemon_sleep, write_behind;
-        int err = -EINVAL;
 
         bitmap->storage.sb_page = alloc_page(GFP_KERNEL);
-        if (IS_ERR(bitmap->storage.sb_page)) {
-                err = PTR_ERR(bitmap->storage.sb_page);
-                bitmap->storage.sb_page = NULL;
-                return err;
-        }
+        if (bitmap->storage.sb_page == NULL)
+                return -ENOMEM;
         bitmap->storage.sb_page->index = 0;
 
         sb = kmap_atomic(bitmap->storage.sb_page);
diff --git a/drivers/md/dm-bio-prison.c b/drivers/md/dm-bio-prison.c
new file mode 100644
index 000000000000..e4e841567459
--- /dev/null
+++ b/drivers/md/dm-bio-prison.c
@@ -0,0 +1,415 @@
+/*
+ * Copyright (C) 2012 Red Hat, Inc.
+ *
+ * This file is released under the GPL.
+ */
+
+#include "dm.h"
+#include "dm-bio-prison.h"
+
+#include <linux/spinlock.h>
+#include <linux/mempool.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+/*----------------------------------------------------------------*/
+
+struct dm_bio_prison_cell {
+        struct hlist_node list;
+        struct dm_bio_prison *prison;
+        struct dm_cell_key key;
+        struct bio *holder;
+        struct bio_list bios;
+};
+
+struct dm_bio_prison {
+        spinlock_t lock;
+        mempool_t *cell_pool;
+
+        unsigned nr_buckets;
+        unsigned hash_mask;
+        struct hlist_head *cells;
+};
+
+/*----------------------------------------------------------------*/
+
+static uint32_t calc_nr_buckets(unsigned nr_cells)
+{
+        uint32_t n = 128;
+
+        nr_cells /= 4;
+        nr_cells = min(nr_cells, 8192u);
+
+        while (n < nr_cells)
+                n <<= 1;
+
+        return n;
+}
+
+static struct kmem_cache *_cell_cache;
+
+/*
+ * @nr_cells should be the number of cells you want in use _concurrently_.
+ * Don't confuse it with the number of distinct keys.
+ */
+struct dm_bio_prison *dm_bio_prison_create(unsigned nr_cells)
+{
+        unsigned i;
+        uint32_t nr_buckets = calc_nr_buckets(nr_cells);
+        size_t len = sizeof(struct dm_bio_prison) +
+                (sizeof(struct hlist_head) * nr_buckets);
+        struct dm_bio_prison *prison = kmalloc(len, GFP_KERNEL);
+
+        if (!prison)
+                return NULL;
+
+        spin_lock_init(&prison->lock);
+        prison->cell_pool = mempool_create_slab_pool(nr_cells, _cell_cache);
+        if (!prison->cell_pool) {
+                kfree(prison);
+                return NULL;
+        }
+
+        prison->nr_buckets = nr_buckets;
+        prison->hash_mask = nr_buckets - 1;
+        prison->cells = (struct hlist_head *) (prison + 1);
+        for (i = 0; i < nr_buckets; i++)
+                INIT_HLIST_HEAD(prison->cells + i);
+
+        return prison;
+}
+EXPORT_SYMBOL_GPL(dm_bio_prison_create);
+
+void dm_bio_prison_destroy(struct dm_bio_prison *prison)
+{
+        mempool_destroy(prison->cell_pool);
+        kfree(prison);
+}
+EXPORT_SYMBOL_GPL(dm_bio_prison_destroy);
+
+static uint32_t hash_key(struct dm_bio_prison *prison, struct dm_cell_key *key)
+{
+        const unsigned long BIG_PRIME = 4294967291UL;
+        uint64_t hash = key->block * BIG_PRIME;
+
+        return (uint32_t) (hash & prison->hash_mask);
+}
+
+static int keys_equal(struct dm_cell_key *lhs, struct dm_cell_key *rhs)
+{
+               return (lhs->virtual == rhs->virtual) &&
+                       (lhs->dev == rhs->dev) &&
+                       (lhs->block == rhs->block);
+}
+
+static struct dm_bio_prison_cell *__search_bucket(struct hlist_head *bucket,
+                                                  struct dm_cell_key *key)
+{
+        struct dm_bio_prison_cell *cell;
+        struct hlist_node *tmp;
+
+        hlist_for_each_entry(cell, tmp, bucket, list)
+                if (keys_equal(&cell->key, key))
+                        return cell;
+
+        return NULL;
+}
+
+/*
+ * This may block if a new cell needs allocating.  You must ensure that
+ * cells will be unlocked even if the calling thread is blocked.
+ *
+ * Returns 1 if the cell was already held, 0 if @inmate is the new holder.
+ */
+int dm_bio_detain(struct dm_bio_prison *prison, struct dm_cell_key *key,
+                  struct bio *inmate, struct dm_bio_prison_cell **ref)
+{
+        int r = 1;
+        unsigned long flags;
+        uint32_t hash = hash_key(prison, key);
+        struct dm_bio_prison_cell *cell, *cell2;
+
+        BUG_ON(hash > prison->nr_buckets);
+
+        spin_lock_irqsave(&prison->lock, flags);
+
+        cell = __search_bucket(prison->cells + hash, key);
+        if (cell) {
+                bio_list_add(&cell->bios, inmate);
+                goto out;
+        }
+
+        /*
+         * Allocate a new cell
+         */
+        spin_unlock_irqrestore(&prison->lock, flags);
+        cell2 = mempool_alloc(prison->cell_pool, GFP_NOIO);
+        spin_lock_irqsave(&prison->lock, flags);
+
+        /*
+         * We've been unlocked, so we have to double check that
+         * nobody else has inserted this cell in the meantime.
+         */
+        cell = __search_bucket(prison->cells + hash, key);
+        if (cell) {
+                mempool_free(cell2, prison->cell_pool);
+                bio_list_add(&cell->bios, inmate);
+                goto out;
+        }
+
+        /*
+         * Use new cell.
+         */
+        cell = cell2;
+
+        cell->prison = prison;
+        memcpy(&cell->key, key, sizeof(cell->key));
+        cell->holder = inmate;
+        bio_list_init(&cell->bios);
+        hlist_add_head(&cell->list, prison->cells + hash);
+
+        r = 0;
+
+out:
+        spin_unlock_irqrestore(&prison->lock, flags);
+
+        *ref = cell;
+
+        return r;
+}
+EXPORT_SYMBOL_GPL(dm_bio_detain);
+
+/*
+ * @inmates must have been initialised prior to this call
+ */
+static void __cell_release(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
+{
+        struct dm_bio_prison *prison = cell->prison;
+
+        hlist_del(&cell->list);
+
+        if (inmates) {
+                bio_list_add(inmates, cell->holder);
+                bio_list_merge(inmates, &cell->bios);
+        }
+
+        mempool_free(cell, prison->cell_pool);
+}
+
+void dm_cell_release(struct dm_bio_prison_cell *cell, struct bio_list *bios)
+{
+        unsigned long flags;
+        struct dm_bio_prison *prison = cell->prison;
+
+        spin_lock_irqsave(&prison->lock, flags);
+        __cell_release(cell, bios);
+        spin_unlock_irqrestore(&prison->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_cell_release);
+
+/*
+ * There are a couple of places where we put a bio into a cell briefly
+ * before taking it out again.  In these situations we know that no other
+ * bio may be in the cell.  This function releases the cell, and also does
+ * a sanity check.
+ */
+static void __cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
+{
+        BUG_ON(cell->holder != bio);
+        BUG_ON(!bio_list_empty(&cell->bios));
+
+        __cell_release(cell, NULL);
+}
+
+void dm_cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
+{
+        unsigned long flags;
+        struct dm_bio_prison *prison = cell->prison;
+
+        spin_lock_irqsave(&prison->lock, flags);
+        __cell_release_singleton(cell, bio);
+        spin_unlock_irqrestore(&prison->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_cell_release_singleton);
+
+/*
+ * Sometimes we don't want the holder, just the additional bios.
+ */
+static void __cell_release_no_holder(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
+{
+        struct dm_bio_prison *prison = cell->prison;
+
+        hlist_del(&cell->list);
+        bio_list_merge(inmates, &cell->bios);
+
+        mempool_free(cell, prison->cell_pool);
+}
+
+void dm_cell_release_no_holder(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
+{
+        unsigned long flags;
+        struct dm_bio_prison *prison = cell->prison;
+
+        spin_lock_irqsave(&prison->lock, flags);
+        __cell_release_no_holder(cell, inmates);
+        spin_unlock_irqrestore(&prison->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_cell_release_no_holder);
+
+void dm_cell_error(struct dm_bio_prison_cell *cell)
+{
+        struct dm_bio_prison *prison = cell->prison;
+        struct bio_list bios;
+        struct bio *bio;
+        unsigned long flags;
+
+        bio_list_init(&bios);
+
+        spin_lock_irqsave(&prison->lock, flags);
+        __cell_release(cell, &bios);
+        spin_unlock_irqrestore(&prison->lock, flags);
+
+        while ((bio = bio_list_pop(&bios)))
+                bio_io_error(bio);
+}
+EXPORT_SYMBOL_GPL(dm_cell_error);
+
+/*----------------------------------------------------------------*/
+
+#define DEFERRED_SET_SIZE 64
+
+struct dm_deferred_entry {
+        struct dm_deferred_set *ds;
+        unsigned count;
+        struct list_head work_items;
+};
+
+struct dm_deferred_set {
+        spinlock_t lock;
+        unsigned current_entry;
+        unsigned sweeper;
+        struct dm_deferred_entry entries[DEFERRED_SET_SIZE];
+};
+
+struct dm_deferred_set *dm_deferred_set_create(void)
+{
+        int i;
+        struct dm_deferred_set *ds;
+
+        ds = kmalloc(sizeof(*ds), GFP_KERNEL);
+        if (!ds)
+                return NULL;
+
+        spin_lock_init(&ds->lock);
+        ds->current_entry = 0;
+        ds->sweeper = 0;
+        for (i = 0; i < DEFERRED_SET_SIZE; i++) {
+                ds->entries[i].ds = ds;
+                ds->entries[i].count = 0;
+                INIT_LIST_HEAD(&ds->entries[i].work_items);
+        }
+
+        return ds;
+}
+EXPORT_SYMBOL_GPL(dm_deferred_set_create);
+
+void dm_deferred_set_destroy(struct dm_deferred_set *ds)
+{
+        kfree(ds);
+}
+EXPORT_SYMBOL_GPL(dm_deferred_set_destroy);
+
+struct dm_deferred_entry *dm_deferred_entry_inc(struct dm_deferred_set *ds)
+{
+        unsigned long flags;
+        struct dm_deferred_entry *entry;
+
+        spin_lock_irqsave(&ds->lock, flags);
+        entry = ds->entries + ds->current_entry;
+        entry->count++;
+        spin_unlock_irqrestore(&ds->lock, flags);
+
+        return entry;
+}
+EXPORT_SYMBOL_GPL(dm_deferred_entry_inc);
+
+static unsigned ds_next(unsigned index)
+{
+        return (index + 1) % DEFERRED_SET_SIZE;
+}
+
+static void __sweep(struct dm_deferred_set *ds, struct list_head *head)
+{
+        while ((ds->sweeper != ds->current_entry) &&
+               !ds->entries[ds->sweeper].count) {
+                list_splice_init(&ds->entries[ds->sweeper].work_items, head);
+                ds->sweeper = ds_next(ds->sweeper);
+        }
+
+        if ((ds->sweeper == ds->current_entry) && !ds->entries[ds->sweeper].count)
+                list_splice_init(&ds->entries[ds->sweeper].work_items, head);
+}
+
+void dm_deferred_entry_dec(struct dm_deferred_entry *entry, struct list_head *head)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&entry->ds->lock, flags);
+        BUG_ON(!entry->count);
+        --entry->count;
+        __sweep(entry->ds, head);
+        spin_unlock_irqrestore(&entry->ds->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_deferred_entry_dec);
+
+/*
+ * Returns 1 if deferred or 0 if no pending items to delay job.
+ */
+int dm_deferred_set_add_work(struct dm_deferred_set *ds, struct list_head *work)
+{
+        int r = 1;
+        unsigned long flags;
+        unsigned next_entry;
+
+        spin_lock_irqsave(&ds->lock, flags);
+        if ((ds->sweeper == ds->current_entry) &&
+            !ds->entries[ds->current_entry].count)
+                r = 0;
+        else {
+                list_add(work, &ds->entries[ds->current_entry].work_items);
+                next_entry = ds_next(ds->current_entry);
+                if (!ds->entries[next_entry].count)
+                        ds->current_entry = next_entry;
+        }
+        spin_unlock_irqrestore(&ds->lock, flags);
+
+        return r;
+}
+EXPORT_SYMBOL_GPL(dm_deferred_set_add_work);
+
+/*----------------------------------------------------------------*/
+
+static int __init dm_bio_prison_init(void)
+{
+        _cell_cache = KMEM_CACHE(dm_bio_prison_cell, 0);
+        if (!_cell_cache)
+                return -ENOMEM;
+
+        return 0;
+}
+
+static void __exit dm_bio_prison_exit(void)
+{
+        kmem_cache_destroy(_cell_cache);
+        _cell_cache = NULL;
+}
+
+/*
+ * module hooks
+ */
+module_init(dm_bio_prison_init);
+module_exit(dm_bio_prison_exit);
+
+MODULE_DESCRIPTION(DM_NAME " bio prison");
+MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/md/dm-bio-prison.h b/drivers/md/dm-bio-prison.h
new file mode 100644
index 000000000000..4e0ac376700a
--- /dev/null
+++ b/drivers/md/dm-bio-prison.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2011-2012 Red Hat, Inc.
+ *
+ * This file is released under the GPL.
+ */
+
+#ifndef DM_BIO_PRISON_H
+#define DM_BIO_PRISON_H
+
+#include "persistent-data/dm-block-manager.h" /* FIXME: for dm_block_t */
+#include "dm-thin-metadata.h" /* FIXME: for dm_thin_id */
+
+#include <linux/list.h>
+#include <linux/bio.h>
+
+/*----------------------------------------------------------------*/
+
+/*
+ * Sometimes we can't deal with a bio straight away.  We put them in prison
+ * where they can't cause any mischief.  Bios are put in a cell identified
+ * by a key, multiple bios can be in the same cell.  When the cell is
+ * subsequently unlocked the bios become available.
+ */
+struct dm_bio_prison;
+struct dm_bio_prison_cell;
+
+/* FIXME: this needs to be more abstract */
+struct dm_cell_key {
+        int virtual;
+        dm_thin_id dev;
+        dm_block_t block;
+};
+
+struct dm_bio_prison *dm_bio_prison_create(unsigned nr_cells);
+void dm_bio_prison_destroy(struct dm_bio_prison *prison);
+
+/*
+ * This may block if a new cell needs allocating.  You must ensure that
+ * cells will be unlocked even if the calling thread is blocked.
+ *
+ * Returns 1 if the cell was already held, 0 if @inmate is the new holder.
+ */
+int dm_bio_detain(struct dm_bio_prison *prison, struct dm_cell_key *key,
+                  struct bio *inmate, struct dm_bio_prison_cell **ref);
+
+void dm_cell_release(struct dm_bio_prison_cell *cell, struct bio_list *bios);
+void dm_cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio); // FIXME: bio arg not needed
+void dm_cell_release_no_holder(struct dm_bio_prison_cell *cell, struct bio_list *inmates);
+void dm_cell_error(struct dm_bio_prison_cell *cell);
+
+/*----------------------------------------------------------------*/
+
+/*
+ * We use the deferred set to keep track of pending reads to shared blocks.
+ * We do this to ensure the new mapping caused by a write isn't performed
+ * until these prior reads have completed.  Otherwise the insertion of the
+ * new mapping could free the old block that the read bios are mapped to.
+ */
+
+struct dm_deferred_set;
+struct dm_deferred_entry;
+
+struct dm_deferred_set *dm_deferred_set_create(void);
+void dm_deferred_set_destroy(struct dm_deferred_set *ds);
+
+struct dm_deferred_entry *dm_deferred_entry_inc(struct dm_deferred_set *ds);
+void dm_deferred_entry_dec(struct dm_deferred_entry *entry, struct list_head *head);
+int dm_deferred_set_add_work(struct dm_deferred_set *ds, struct list_head *work);
+
+/*----------------------------------------------------------------*/
+
+#endif
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index cc06a1e52423..651ca79881dd 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -280,9 +280,7 @@ static void __cache_size_refresh(void)
         BUG_ON(!mutex_is_locked(&dm_bufio_clients_lock));
         BUG_ON(dm_bufio_client_count < 0);
 
-        dm_bufio_cache_size_latch = dm_bufio_cache_size;
-
-        barrier();
+        dm_bufio_cache_size_latch = ACCESS_ONCE(dm_bufio_cache_size);
 
         /*
          * Use default if set to 0 and report the actual cache size used.
@@ -441,8 +439,7 @@ static void __relink_lru(struct dm_buffer *b, int dirty)
         c->n_buffers[b->list_mode]--;
         c->n_buffers[dirty]++;
         b->list_mode = dirty;
-        list_del(&b->lru_list);
-        list_add(&b->lru_list, &c->lru[dirty]);
+        list_move(&b->lru_list, &c->lru[dirty]);
 }
 
 /*----------------------------------------------------------------
@@ -813,7 +810,7 @@ static void __get_memory_limit(struct dm_bufio_client *c,
 {
         unsigned long buffers;
 
-        if (dm_bufio_cache_size != dm_bufio_cache_size_latch) {
+        if (ACCESS_ONCE(dm_bufio_cache_size) != dm_bufio_cache_size_latch) {
                 mutex_lock(&dm_bufio_clients_lock);
                 __cache_size_refresh();
                 mutex_unlock(&dm_bufio_clients_lock);
@@ -1591,11 +1588,9 @@ EXPORT_SYMBOL_GPL(dm_bufio_client_destroy);
 
 static void cleanup_old_buffers(void)
 {
-        unsigned long max_age = dm_bufio_max_age;
+        unsigned long max_age = ACCESS_ONCE(dm_bufio_max_age);
         struct dm_bufio_client *c;
 
-        barrier();
-
         if (max_age > ULONG_MAX / HZ)
                 max_age = ULONG_MAX / HZ;
 
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 664743d6a6cd..bbf459bca61d 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -798,14 +798,6 @@ static int crypt_convert(struct crypt_config *cc,
         return 0;
 }
 
-static void dm_crypt_bio_destructor(struct bio *bio)
-{
-        struct dm_crypt_io *io = bio->bi_private;
-        struct crypt_config *cc = io->cc;
-
-        bio_free(bio, cc->bs);
-}
-
 /*
  * Generate a new unfragmented bio with the given size
  * This should never violate the device limitations
@@ -974,7 +966,6 @@ static void clone_init(struct dm_crypt_io *io, struct bio *clone)
         clone->bi_end_io  = crypt_endio;
         clone->bi_bdev    = cc->dev->bdev;
         clone->bi_rw      = io->base_bio->bi_rw;
-        clone->bi_destructor = dm_crypt_bio_destructor;
 }
 
 static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
@@ -988,19 +979,14 @@ static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
          * copy the required bvecs because we need the original
          * one in order to decrypt the whole bio data *afterwards*.
          */
-        clone = bio_alloc_bioset(gfp, bio_segments(base_bio), cc->bs);
+        clone = bio_clone_bioset(base_bio, gfp, cc->bs);
         if (!clone)
                 return 1;
 
         crypt_inc_pending(io);
 
         clone_init(io, clone);
-        clone->bi_idx = 0;
-        clone->bi_vcnt = bio_segments(base_bio);
-        clone->bi_size = base_bio->bi_size;
         clone->bi_sector = cc->start + io->sector;
-        memcpy(clone->bi_io_vec, bio_iovec(base_bio),
-               sizeof(struct bio_vec) * clone->bi_vcnt);
 
         generic_make_request(clone);
         return 0;
diff --git a/drivers/md/dm-io.c b/drivers/md/dm-io.c
index ea5dd289fe2a..1c46f97d6664 100644
--- a/drivers/md/dm-io.c
+++ b/drivers/md/dm-io.c
@@ -249,16 +249,6 @@ static void vm_dp_init(struct dpages *dp, void *data)
         dp->context_ptr = data;
 }
 
-static void dm_bio_destructor(struct bio *bio)
-{
-        unsigned region;
-        struct io *io;
-
-        retrieve_io_and_region_from_bio(bio, &io, &region);
-
-        bio_free(bio, io->client->bios);
-}
-
 /*
  * Functions for getting the pages from kernel memory.
  */
@@ -317,7 +307,6 @@ static void do_region(int rw, unsigned region, struct dm_io_region *where,
                 bio->bi_sector = where->sector + (where->count - remaining);
                 bio->bi_bdev = where->bdev;
                 bio->bi_end_io = endio;
-                bio->bi_destructor = dm_bio_destructor;
                 store_io_and_region_in_bio(bio, io, region);
 
                 if (rw & REQ_DISCARD) {
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index d778563a4ffd..573bd04591bf 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -1309,13 +1309,14 @@ static int multipath_end_io(struct dm_target *ti, struct request *clone,
 {
         struct multipath *m = ti->private;
         struct dm_mpath_io *mpio = map_context->ptr;
-        struct pgpath *pgpath = mpio->pgpath;
+        struct pgpath *pgpath;
         struct path_selector *ps;
         int r;
 
         BUG_ON(!mpio);
 
         r  = do_end_io(m, clone, error, mpio);
+        pgpath = mpio->pgpath;
         if (pgpath) {
                 ps = &pgpath->pg->ps;
                 if (ps->type->end_io)
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 982e3e390c45..45d94a7e7f6d 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -338,6 +338,84 @@ static int validate_region_size(struct raid_set *rs, unsigned long region_size)
 }
 
 /*
+ * validate_rebuild_devices
+ * @rs
+ *
+ * Determine if the devices specified for rebuild can result in a valid
+ * usable array that is capable of rebuilding the given devices.
+ *
+ * Returns: 0 on success, -EINVAL on failure.
+ */
+static int validate_rebuild_devices(struct raid_set *rs)
+{
+        unsigned i, rebuild_cnt = 0;
+        unsigned rebuilds_per_group, copies, d;
+
+        if (!(rs->print_flags & DMPF_REBUILD))
+                return 0;
+
+        for (i = 0; i < rs->md.raid_disks; i++)
+                if (!test_bit(In_sync, &rs->dev[i].rdev.flags))
+                        rebuild_cnt++;
+
+        switch (rs->raid_type->level) {
+        case 1:
+                if (rebuild_cnt >= rs->md.raid_disks)
+                        goto too_many;
+                break;
+        case 4:
+        case 5:
+        case 6:
+                if (rebuild_cnt > rs->raid_type->parity_devs)
+                        goto too_many;
+                break;
+        case 10:
+                copies = raid10_md_layout_to_copies(rs->md.layout);
+                if (rebuild_cnt < copies)
+                        break;
+
+                /*
+                 * It is possible to have a higher rebuild count for RAID10,
+                 * as long as the failed devices occur in different mirror
+                 * groups (i.e. different stripes).
+                 *
+                 * Right now, we only allow for "near" copies.  When other
+                 * formats are added, we will have to check those too.
+                 *
+                 * When checking "near" format, make sure no adjacent devices
+                 * have failed beyond what can be handled.  In addition to the
+                 * simple case where the number of devices is a multiple of the
+                 * number of copies, we must also handle cases where the number
+                 * of devices is not a multiple of the number of copies.
+                 * E.g.    dev1 dev2 dev3 dev4 dev5
+                 *          A    A    B    B    C
+                 *          C    D    D    E    E
+                 */
+                rebuilds_per_group = 0;
+                for (i = 0; i < rs->md.raid_disks * copies; i++) {
+                        d = i % rs->md.raid_disks;
+                        if (!test_bit(In_sync, &rs->dev[d].rdev.flags) &&
+                            (++rebuilds_per_group >= copies))
+                                goto too_many;
+                        if (!((i + 1) % copies))
+                                rebuilds_per_group = 0;
+                }
+                break;
+        default:
+                DMERR("The rebuild parameter is not supported for %s",
+                      rs->raid_type->name);
+                rs->ti->error = "Rebuild not supported for this RAID type";
+                return -EINVAL;
+        }
+
+        return 0;
+
+too_many:
+        rs->ti->error = "Too many rebuild devices specified";
+        return -EINVAL;
+}
+
+/*
  * Possible arguments are...
  *      <chunk_size> [optional_args]
  *
@@ -365,7 +443,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 {
         char *raid10_format = "near";
         unsigned raid10_copies = 2;
-        unsigned i, rebuild_cnt = 0;
+        unsigned i;
         unsigned long value, region_size = 0;
         sector_t sectors_per_dev = rs->ti->len;
         sector_t max_io_len;
@@ -461,31 +539,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 
                 /* Parameters that take a numeric value are checked here */
                 if (!strcasecmp(key, "rebuild")) {
-                        rebuild_cnt++;
-
-                        switch (rs->raid_type->level) {
-                        case 1:
-                                if (rebuild_cnt >= rs->md.raid_disks) {
-                                        rs->ti->error = "Too many rebuild devices specified";
-                                        return -EINVAL;
-                                }
-                                break;
-                        case 4:
-                        case 5:
-                        case 6:
-                                if (rebuild_cnt > rs->raid_type->parity_devs) {
-                                        rs->ti->error = "Too many rebuild devices specified for given RAID type";
-                                        return -EINVAL;
-                                }
-                                break;
-                        case 10:
-                        default:
-                                DMERR("The rebuild parameter is not supported for %s", rs->raid_type->name);
-                                rs->ti->error = "Rebuild not supported for this RAID type";
-                                return -EINVAL;
-                        }
-
-                        if (value > rs->md.raid_disks) {
+                        if (value >= rs->md.raid_disks) {
                                 rs->ti->error = "Invalid rebuild index given";
                                 return -EINVAL;
                         }
@@ -608,6 +662,9 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
         }
         rs->md.dev_sectors = sectors_per_dev;
 
+        if (validate_rebuild_devices(rs))
+                return -EINVAL;
+
         /* Assume there are no metadata devices until the drives are parsed */
         rs->md.persistent = 0;
         rs->md.external = 1;
@@ -960,6 +1017,19 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 
         freshest = NULL;
         rdev_for_each_safe(rdev, tmp, mddev) {
+                /*
+                 * Skipping super_load due to DMPF_SYNC will cause
+                 * the array to undergo initialization again as
+                 * though it were new.  This is the intended effect
+                 * of the "sync" directive.
+                 *
+                 * When reshaping capability is added, we must ensure
+                 * that the "sync" directive is disallowed during the
+                 * reshape.
+                 */
+                if (rs->print_flags & DMPF_SYNC)
+                        continue;
+
                 if (!rdev->meta_bdev)
                         continue;
 
@@ -1360,7 +1430,7 @@ static void raid_resume(struct dm_target *ti)
 
 static struct target_type raid_target = {
         .name = "raid",
-        .version = {1, 3, 0},
+        .version = {1, 3, 1},
         .module = THIS_MODULE,
         .ctr = raid_ctr,
         .dtr = raid_dtr,
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index c29410af1e22..058acf3a5ba7 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -5,6 +5,7 @@
  */
 
 #include "dm-thin-metadata.h"
+#include "dm-bio-prison.h"
 #include "dm.h"
 
 #include <linux/device-mapper.h>
@@ -21,7 +22,6 @@
  * Tunable constants
  */
 #define ENDIO_HOOK_POOL_SIZE 1024
-#define DEFERRED_SET_SIZE 64
 #define MAPPING_POOL_SIZE 1024
 #define PRISON_CELLS 1024
 #define COMMIT_PERIOD HZ
@@ -58,7 +58,7 @@
  * i) plug io further to this physical block. (see bio_prison code).
  *
  * ii) quiesce any read io to that shared data block.  Obviously
- * including all devices that share this block.  (see deferred_set code)
+ * including all devices that share this block.  (see dm_deferred_set code)
  *
  * iii) copy the data block to a newly allocate block.  This step can be
  * missed out if the io covers the block. (schedule_copy).
@@ -99,381 +99,10 @@
 /*----------------------------------------------------------------*/
 
 /*
- * Sometimes we can't deal with a bio straight away.  We put them in prison
- * where they can't cause any mischief.  Bios are put in a cell identified
- * by a key, multiple bios can be in the same cell.  When the cell is
- * subsequently unlocked the bios become available.
- */
-struct bio_prison;
-
-struct cell_key {
-        int virtual;
-        dm_thin_id dev;
-        dm_block_t block;
-};
-
-struct dm_bio_prison_cell {
-        struct hlist_node list;
-        struct bio_prison *prison;
-        struct cell_key key;
-        struct bio *holder;
-        struct bio_list bios;
-};
-
-struct bio_prison {
-        spinlock_t lock;
-        mempool_t *cell_pool;
-
-        unsigned nr_buckets;
-        unsigned hash_mask;
-        struct hlist_head *cells;
-};
-
-static uint32_t calc_nr_buckets(unsigned nr_cells)
-{
-        uint32_t n = 128;
-
-        nr_cells /= 4;
-        nr_cells = min(nr_cells, 8192u);
-
-        while (n < nr_cells)
-                n <<= 1;
-
-        return n;
-}
-
-static struct kmem_cache *_cell_cache;
-
-/*
- * @nr_cells should be the number of cells you want in use _concurrently_.
- * Don't confuse it with the number of distinct keys.
- */
-static struct bio_prison *prison_create(unsigned nr_cells)
-{
-        unsigned i;
-        uint32_t nr_buckets = calc_nr_buckets(nr_cells);
-        size_t len = sizeof(struct bio_prison) +
-                (sizeof(struct hlist_head) * nr_buckets);
-        struct bio_prison *prison = kmalloc(len, GFP_KERNEL);
-
-        if (!prison)
-                return NULL;
-
-        spin_lock_init(&prison->lock);
-        prison->cell_pool = mempool_create_slab_pool(nr_cells, _cell_cache);
-        if (!prison->cell_pool) {
-                kfree(prison);
-                return NULL;
-        }
-
-        prison->nr_buckets = nr_buckets;
-        prison->hash_mask = nr_buckets - 1;
-        prison->cells = (struct hlist_head *) (prison + 1);
-        for (i = 0; i < nr_buckets; i++)
-                INIT_HLIST_HEAD(prison->cells + i);
-
-        return prison;
-}
-
-static void prison_destroy(struct bio_prison *prison)
-{
-        mempool_destroy(prison->cell_pool);
-        kfree(prison);
-}
-
-static uint32_t hash_key(struct bio_prison *prison, struct cell_key *key)
-{
-        const unsigned long BIG_PRIME = 4294967291UL;
-        uint64_t hash = key->block * BIG_PRIME;
-
-        return (uint32_t) (hash & prison->hash_mask);
-}
-
-static int keys_equal(struct cell_key *lhs, struct cell_key *rhs)
-{
-               return (lhs->virtual == rhs->virtual) &&
-                       (lhs->dev == rhs->dev) &&
-                       (lhs->block == rhs->block);
-}
-
-static struct dm_bio_prison_cell *__search_bucket(struct hlist_head *bucket,
-                                                  struct cell_key *key)
-{
-        struct dm_bio_prison_cell *cell;
-        struct hlist_node *tmp;
-
-        hlist_for_each_entry(cell, tmp, bucket, list)
-                if (keys_equal(&cell->key, key))
-                        return cell;
-
-        return NULL;
-}
-
-/*
- * This may block if a new cell needs allocating.  You must ensure that
- * cells will be unlocked even if the calling thread is blocked.
- *
- * Returns 1 if the cell was already held, 0 if @inmate is the new holder.
- */
-static int bio_detain(struct bio_prison *prison, struct cell_key *key,
-                      struct bio *inmate, struct dm_bio_prison_cell **ref)
-{
-        int r = 1;
-        unsigned long flags;
-        uint32_t hash = hash_key(prison, key);
-        struct dm_bio_prison_cell *cell, *cell2;
-
-        BUG_ON(hash > prison->nr_buckets);
-
-        spin_lock_irqsave(&prison->lock, flags);
-
-        cell = __search_bucket(prison->cells + hash, key);
-        if (cell) {
-                bio_list_add(&cell->bios, inmate);
-                goto out;
-        }
-
-        /*
-         * Allocate a new cell
-         */
-        spin_unlock_irqrestore(&prison->lock, flags);
-        cell2 = mempool_alloc(prison->cell_pool, GFP_NOIO);
-        spin_lock_irqsave(&prison->lock, flags);
-
-        /*
-         * We've been unlocked, so we have to double check that
-         * nobody else has inserted this cell in the meantime.
-         */
-        cell = __search_bucket(prison->cells + hash, key);
-        if (cell) {
-                mempool_free(cell2, prison->cell_pool);
-                bio_list_add(&cell->bios, inmate);
-                goto out;
-        }
-
-        /*
-         * Use new cell.
-         */
-        cell = cell2;
-
-        cell->prison = prison;
-        memcpy(&cell->key, key, sizeof(cell->key));
-        cell->holder = inmate;
-        bio_list_init(&cell->bios);
-        hlist_add_head(&cell->list, prison->cells + hash);
-
-        r = 0;
-
-out:
-        spin_unlock_irqrestore(&prison->lock, flags);
-
-        *ref = cell;
-
-        return r;
-}
-
-/*
- * @inmates must have been initialised prior to this call
- */
-static void __cell_release(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
-{
-        struct bio_prison *prison = cell->prison;
-
-        hlist_del(&cell->list);
-
-        if (inmates) {
-                bio_list_add(inmates, cell->holder);
-                bio_list_merge(inmates, &cell->bios);
-        }
-
-        mempool_free(cell, prison->cell_pool);
-}
-
-static void cell_release(struct dm_bio_prison_cell *cell, struct bio_list *bios)
-{
-        unsigned long flags;
-        struct bio_prison *prison = cell->prison;
-
-        spin_lock_irqsave(&prison->lock, flags);
-        __cell_release(cell, bios);
-        spin_unlock_irqrestore(&prison->lock, flags);
-}
-
-/*
- * There are a couple of places where we put a bio into a cell briefly
- * before taking it out again.  In these situations we know that no other
- * bio may be in the cell.  This function releases the cell, and also does
- * a sanity check.
- */
-static void __cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
-{
-        BUG_ON(cell->holder != bio);
-        BUG_ON(!bio_list_empty(&cell->bios));
-
-        __cell_release(cell, NULL);
-}
-
-static void cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
-{
-        unsigned long flags;
-        struct bio_prison *prison = cell->prison;
-
-        spin_lock_irqsave(&prison->lock, flags);
-        __cell_release_singleton(cell, bio);
-        spin_unlock_irqrestore(&prison->lock, flags);
-}
-
-/*
- * Sometimes we don't want the holder, just the additional bios.
- */
-static void __cell_release_no_holder(struct dm_bio_prison_cell *cell,
-                                     struct bio_list *inmates)
-{
-        struct bio_prison *prison = cell->prison;
-
-        hlist_del(&cell->list);
-        bio_list_merge(inmates, &cell->bios);
-
-        mempool_free(cell, prison->cell_pool);
-}
-
-static void cell_release_no_holder(struct dm_bio_prison_cell *cell,
-                                   struct bio_list *inmates)
-{
-        unsigned long flags;
-        struct bio_prison *prison = cell->prison;
-
-        spin_lock_irqsave(&prison->lock, flags);
-        __cell_release_no_holder(cell, inmates);
-        spin_unlock_irqrestore(&prison->lock, flags);
-}
-
-static void cell_error(struct dm_bio_prison_cell *cell)
-{
-        struct bio_prison *prison = cell->prison;
-        struct bio_list bios;
-        struct bio *bio;
-        unsigned long flags;
-
-        bio_list_init(&bios);
-
-        spin_lock_irqsave(&prison->lock, flags);
-        __cell_release(cell, &bios);
-        spin_unlock_irqrestore(&prison->lock, flags);
-
-        while ((bio = bio_list_pop(&bios)))
-                bio_io_error(bio);
-}
-
-/*----------------------------------------------------------------*/
-
-/*
- * We use the deferred set to keep track of pending reads to shared blocks.
- * We do this to ensure the new mapping caused by a write isn't performed
- * until these prior reads have completed.  Otherwise the insertion of the
- * new mapping could free the old block that the read bios are mapped to.
- */
-
-struct deferred_set;
-struct deferred_entry {
-        struct deferred_set *ds;
-        unsigned count;
-        struct list_head work_items;
-};
-
-struct deferred_set {
-        spinlock_t lock;
-        unsigned current_entry;
-        unsigned sweeper;
-        struct deferred_entry entries[DEFERRED_SET_SIZE];
-};
-
-static void ds_init(struct deferred_set *ds)
-{
-        int i;
-
-        spin_lock_init(&ds->lock);
-        ds->current_entry = 0;
-        ds->sweeper = 0;
-        for (i = 0; i < DEFERRED_SET_SIZE; i++) {
-                ds->entries[i].ds = ds;
-                ds->entries[i].count = 0;
-                INIT_LIST_HEAD(&ds->entries[i].work_items);
-        }
-}
-
-static struct deferred_entry *ds_inc(struct deferred_set *ds)
-{
-        unsigned long flags;
-        struct deferred_entry *entry;
-
-        spin_lock_irqsave(&ds->lock, flags);
-        entry = ds->entries + ds->current_entry;
-        entry->count++;
-        spin_unlock_irqrestore(&ds->lock, flags);
-
-        return entry;
-}
-
-static unsigned ds_next(unsigned index)
-{
-        return (index + 1) % DEFERRED_SET_SIZE;
-}
-
-static void __sweep(struct deferred_set *ds, struct list_head *head)
-{
-        while ((ds->sweeper != ds->current_entry) &&
-               !ds->entries[ds->sweeper].count) {
-                list_splice_init(&ds->entries[ds->sweeper].work_items, head);
-                ds->sweeper = ds_next(ds->sweeper);
-        }
-
-        if ((ds->sweeper == ds->current_entry) && !ds->entries[ds->sweeper].count)
-                list_splice_init(&ds->entries[ds->sweeper].work_items, head);
-}
-
-static void ds_dec(struct deferred_entry *entry, struct list_head *head)
-{
-        unsigned long flags;
-
-        spin_lock_irqsave(&entry->ds->lock, flags);
-        BUG_ON(!entry->count);
-        --entry->count;
-        __sweep(entry->ds, head);
-        spin_unlock_irqrestore(&entry->ds->lock, flags);
-}
-
-/*
- * Returns 1 if deferred or 0 if no pending items to delay job.
- */
-static int ds_add_work(struct deferred_set *ds, struct list_head *work)
-{
-        int r = 1;
-        unsigned long flags;
-        unsigned next_entry;
-
-        spin_lock_irqsave(&ds->lock, flags);
-        if ((ds->sweeper == ds->current_entry) &&
-            !ds->entries[ds->current_entry].count)
-                r = 0;
-        else {
-                list_add(work, &ds->entries[ds->current_entry].work_items);
-                next_entry = ds_next(ds->current_entry);
-                if (!ds->entries[next_entry].count)
-                        ds->current_entry = next_entry;
-        }
-        spin_unlock_irqrestore(&ds->lock, flags);
-
-        return r;
-}
-
-/*----------------------------------------------------------------*/
-
-/*
  * Key building.
  */
 static void build_data_key(struct dm_thin_device *td,
-                           dm_block_t b, struct cell_key *key)
+                           dm_block_t b, struct dm_cell_key *key)
 {
         key->virtual = 0;
         key->dev = dm_thin_dev_id(td);
@@ -481,7 +110,7 @@ static void build_data_key(struct dm_thin_device *td,
 }
 
 static void build_virtual_key(struct dm_thin_device *td, dm_block_t b,
-                              struct cell_key *key)
+                              struct dm_cell_key *key)
 {
         key->virtual = 1;
         key->dev = dm_thin_dev_id(td);
@@ -534,7 +163,7 @@ struct pool {
         unsigned low_water_triggered:1; /* A dm event has been sent */
         unsigned no_free_space:1;       /* A -ENOSPC warning has been issued */
 
-        struct bio_prison *prison;
+        struct dm_bio_prison *prison;
         struct dm_kcopyd_client *copier;
 
         struct workqueue_struct *wq;
@@ -552,8 +181,8 @@ struct pool {
 
         struct bio_list retry_on_resume_list;
 
-        struct deferred_set shared_read_ds;
-        struct deferred_set all_io_ds;
+        struct dm_deferred_set *shared_read_ds;
+        struct dm_deferred_set *all_io_ds;
 
         struct dm_thin_new_mapping *next_mapping;
         mempool_t *mapping_pool;
@@ -660,8 +289,8 @@ static struct pool *__pool_table_lookup_metadata_dev(struct block_device *md_dev
 
 struct dm_thin_endio_hook {
         struct thin_c *tc;
-        struct deferred_entry *shared_read_entry;
-        struct deferred_entry *all_io_entry;
+        struct dm_deferred_entry *shared_read_entry;
+        struct dm_deferred_entry *all_io_entry;
         struct dm_thin_new_mapping *overwrite_mapping;
 };
 
@@ -877,7 +506,7 @@ static void cell_defer(struct thin_c *tc, struct dm_bio_prison_cell *cell,
         unsigned long flags;
 
         spin_lock_irqsave(&pool->lock, flags);
-        cell_release(cell, &pool->deferred_bios);
+        dm_cell_release(cell, &pool->deferred_bios);
         spin_unlock_irqrestore(&tc->pool->lock, flags);
 
         wake_worker(pool);
@@ -896,7 +525,7 @@ static void cell_defer_except(struct thin_c *tc, struct dm_bio_prison_cell *cell
         bio_list_init(&bios);
 
         spin_lock_irqsave(&pool->lock, flags);
-        cell_release_no_holder(cell, &pool->deferred_bios);
+        dm_cell_release_no_holder(cell, &pool->deferred_bios);
         spin_unlock_irqrestore(&pool->lock, flags);
 
         wake_worker(pool);
@@ -906,7 +535,7 @@ static void process_prepared_mapping_fail(struct dm_thin_new_mapping *m)
 {
         if (m->bio)
                 m->bio->bi_end_io = m->saved_bi_end_io;
-        cell_error(m->cell);
+        dm_cell_error(m->cell);
         list_del(&m->list);
         mempool_free(m, m->tc->pool->mapping_pool);
 }
@@ -921,7 +550,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
                 bio->bi_end_io = m->saved_bi_end_io;
 
         if (m->err) {
-                cell_error(m->cell);
+                dm_cell_error(m->cell);
                 goto out;
         }
 
@@ -933,7 +562,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
         r = dm_thin_insert_block(tc->td, m->virt_block, m->data_block);
         if (r) {
                 DMERR("dm_thin_insert_block() failed");
-                cell_error(m->cell);
+                dm_cell_error(m->cell);
                 goto out;
         }
 
@@ -1067,7 +696,7 @@ static void schedule_copy(struct thin_c *tc, dm_block_t virt_block,
         m->err = 0;
         m->bio = NULL;
 
-        if (!ds_add_work(&pool->shared_read_ds, &m->list))
+        if (!dm_deferred_set_add_work(pool->shared_read_ds, &m->list))
                 m->quiesced = 1;
 
         /*
@@ -1099,7 +728,7 @@ static void schedule_copy(struct thin_c *tc, dm_block_t virt_block,
                 if (r < 0) {
                         mempool_free(m, pool->mapping_pool);
                         DMERR("dm_kcopyd_copy() failed");
-                        cell_error(cell);
+                        dm_cell_error(cell);
                 }
         }
 }
@@ -1164,7 +793,7 @@ static void schedule_zero(struct thin_c *tc, dm_block_t virt_block,
                 if (r < 0) {
                         mempool_free(m, pool->mapping_pool);
                         DMERR("dm_kcopyd_zero() failed");
-                        cell_error(cell);
+                        dm_cell_error(cell);
                 }
         }
 }
@@ -1276,7 +905,7 @@ static void no_space(struct dm_bio_prison_cell *cell)
         struct bio_list bios;
 
         bio_list_init(&bios);
-        cell_release(cell, &bios);
+        dm_cell_release(cell, &bios);
 
         while ((bio = bio_list_pop(&bios)))
                 retry_on_resume(bio);
@@ -1288,13 +917,13 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
         unsigned long flags;
         struct pool *pool = tc->pool;
         struct dm_bio_prison_cell *cell, *cell2;
-        struct cell_key key, key2;
+        struct dm_cell_key key, key2;
         dm_block_t block = get_bio_block(tc, bio);
         struct dm_thin_lookup_result lookup_result;
         struct dm_thin_new_mapping *m;
 
         build_virtual_key(tc->td, block, &key);
-        if (bio_detain(tc->pool->prison, &key, bio, &cell))
+        if (dm_bio_detain(tc->pool->prison, &key, bio, &cell))
                 return;
 
         r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
@@ -1306,8 +935,8 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
                  * on this block.
                  */
                 build_data_key(tc->td, lookup_result.block, &key2);
-                if (bio_detain(tc->pool->prison, &key2, bio, &cell2)) {
-                        cell_release_singleton(cell, bio);
+                if (dm_bio_detain(tc->pool->prison, &key2, bio, &cell2)) {
+                        dm_cell_release_singleton(cell, bio);
                         break;
                 }
 
@@ -1326,7 +955,7 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
                         m->err = 0;
                         m->bio = bio;
 
-                        if (!ds_add_work(&pool->all_io_ds, &m->list)) {
+                        if (!dm_deferred_set_add_work(pool->all_io_ds, &m->list)) {
                                 spin_lock_irqsave(&pool->lock, flags);
                                 list_add(&m->list, &pool->prepared_discards);
                                 spin_unlock_irqrestore(&pool->lock, flags);
@@ -1338,8 +967,8 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
                          * a block boundary.  So we submit the discard of a
                          * partial block appropriately.
                          */
-                        cell_release_singleton(cell, bio);
-                        cell_release_singleton(cell2, bio);
+                        dm_cell_release_singleton(cell, bio);
+                        dm_cell_release_singleton(cell2, bio);
                         if ((!lookup_result.shared) && pool->pf.discard_passdown)
                                 remap_and_issue(tc, bio, lookup_result.block);
                         else
@@ -1351,20 +980,20 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
                 /*
                  * It isn't provisioned, just forget it.
                  */
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
                 bio_endio(bio, 0);
                 break;
 
         default:
                 DMERR("discard: find block unexpectedly returned %d", r);
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
                 bio_io_error(bio);
                 break;
         }
 }
 
 static void break_sharing(struct thin_c *tc, struct bio *bio, dm_block_t block,
-                          struct cell_key *key,
+                          struct dm_cell_key *key,
                           struct dm_thin_lookup_result *lookup_result,
                           struct dm_bio_prison_cell *cell)
 {
@@ -1384,7 +1013,7 @@ static void break_sharing(struct thin_c *tc, struct bio *bio, dm_block_t block,
 
         default:
                 DMERR("%s: alloc_data_block() failed, error = %d", __func__, r);
-                cell_error(cell);
+                dm_cell_error(cell);
                 break;
         }
 }
@@ -1395,14 +1024,14 @@ static void process_shared_bio(struct thin_c *tc, struct bio *bio,
 {
         struct dm_bio_prison_cell *cell;
         struct pool *pool = tc->pool;
-        struct cell_key key;
+        struct dm_cell_key key;
 
         /*
          * If cell is already occupied, then sharing is already in the process
          * of being broken so we have nothing further to do here.
          */
         build_data_key(tc->td, lookup_result->block, &key);
-        if (bio_detain(pool->prison, &key, bio, &cell))
+        if (dm_bio_detain(pool->prison, &key, bio, &cell))
                 return;
 
         if (bio_data_dir(bio) == WRITE && bio->bi_size)
@@ -1410,9 +1039,9 @@ static void process_shared_bio(struct thin_c *tc, struct bio *bio,
         else {
                 struct dm_thin_endio_hook *h = dm_get_mapinfo(bio)->ptr;
 
-                h->shared_read_entry = ds_inc(&pool->shared_read_ds);
+                h->shared_read_entry = dm_deferred_entry_inc(pool->shared_read_ds);
 
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
                 remap_and_issue(tc, bio, lookup_result->block);
         }
 }
@@ -1427,7 +1056,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
          * Remap empty bios (flushes) immediately, without provisioning.
          */
         if (!bio->bi_size) {
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
                 remap_and_issue(tc, bio, 0);
                 return;
         }
@@ -1437,7 +1066,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
          */
         if (bio_data_dir(bio) == READ) {
                 zero_fill_bio(bio);
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
                 bio_endio(bio, 0);
                 return;
         }
@@ -1458,7 +1087,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
         default:
                 DMERR("%s: alloc_data_block() failed, error = %d", __func__, r);
                 set_pool_mode(tc->pool, PM_READ_ONLY);
-                cell_error(cell);
+                dm_cell_error(cell);
                 break;
         }
 }
@@ -1468,7 +1097,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
         int r;
         dm_block_t block = get_bio_block(tc, bio);
         struct dm_bio_prison_cell *cell;
-        struct cell_key key;
+        struct dm_cell_key key;
         struct dm_thin_lookup_result lookup_result;
 
         /*
@@ -1476,7 +1105,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
          * being provisioned so we have nothing further to do here.
          */
         build_virtual_key(tc->td, block, &key);
-        if (bio_detain(tc->pool->prison, &key, bio, &cell))
+        if (dm_bio_detain(tc->pool->prison, &key, bio, &cell))
                 return;
 
         r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
@@ -1491,7 +1120,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
                  * TODO: this will probably have to change when discard goes
                  * back in.
                  */
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
 
                 if (lookup_result.shared)
                         process_shared_bio(tc, bio, block, &lookup_result);
@@ -1501,7 +1130,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 
         case -ENODATA:
                 if (bio_data_dir(bio) == READ && tc->origin_dev) {
-                        cell_release_singleton(cell, bio);
+                        dm_cell_release_singleton(cell, bio);
                         remap_to_origin_and_issue(tc, bio);
                 } else
                         provision_block(tc, bio, block, cell);
@@ -1509,7 +1138,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 
         default:
                 DMERR("dm_thin_find_block() failed, error = %d", r);
-                cell_release_singleton(cell, bio);
+                dm_cell_release_singleton(cell, bio);
                 bio_io_error(bio);
                 break;
         }
@@ -1718,7 +1347,7 @@ static struct dm_thin_endio_hook *thin_hook_bio(struct thin_c *tc, struct bio *b
 
         h->tc = tc;
         h->shared_read_entry = NULL;
-        h->all_io_entry = bio->bi_rw & REQ_DISCARD ? NULL : ds_inc(&pool->all_io_ds);
+        h->all_io_entry = bio->bi_rw & REQ_DISCARD ? NULL : dm_deferred_entry_inc(pool->all_io_ds);
         h->overwrite_mapping = NULL;
 
         return h;
@@ -1928,7 +1557,7 @@ static void __pool_destroy(struct pool *pool)
         if (dm_pool_metadata_close(pool->pmd) < 0)
                 DMWARN("%s: dm_pool_metadata_close() failed.", __func__);
 
-        prison_destroy(pool->prison);
+        dm_bio_prison_destroy(pool->prison);
         dm_kcopyd_client_destroy(pool->copier);
 
         if (pool->wq)
@@ -1938,6 +1567,8 @@ static void __pool_destroy(struct pool *pool)
                 mempool_free(pool->next_mapping, pool->mapping_pool);
         mempool_destroy(pool->mapping_pool);
         mempool_destroy(pool->endio_hook_pool);
+        dm_deferred_set_destroy(pool->shared_read_ds);
+        dm_deferred_set_destroy(pool->all_io_ds);
         kfree(pool);
 }
 
@@ -1976,7 +1607,7 @@ static struct pool *pool_create(struct mapped_device *pool_md,
                 pool->sectors_per_block_shift = __ffs(block_size);
         pool->low_water_blocks = 0;
         pool_features_init(&pool->pf);
-        pool->prison = prison_create(PRISON_CELLS);
+        pool->prison = dm_bio_prison_create(PRISON_CELLS);
         if (!pool->prison) {
                 *error = "Error creating pool's bio prison";
                 err_p = ERR_PTR(-ENOMEM);
@@ -2012,8 +1643,20 @@ static struct pool *pool_create(struct mapped_device *pool_md,
         pool->low_water_triggered = 0;
         pool->no_free_space = 0;
         bio_list_init(&pool->retry_on_resume_list);
-        ds_init(&pool->shared_read_ds);
-        ds_init(&pool->all_io_ds);
+
+        pool->shared_read_ds = dm_deferred_set_create();
+        if (!pool->shared_read_ds) {
+                *error = "Error creating pool's shared read deferred set";
+                err_p = ERR_PTR(-ENOMEM);
+                goto bad_shared_read_ds;
+        }
+
+        pool->all_io_ds = dm_deferred_set_create();
+        if (!pool->all_io_ds) {
+                *error = "Error creating pool's all io deferred set";
+                err_p = ERR_PTR(-ENOMEM);
+                goto bad_all_io_ds;
+        }
 
         pool->next_mapping = NULL;
         pool->mapping_pool = mempool_create_slab_pool(MAPPING_POOL_SIZE,
@@ -2042,11 +1685,15 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 bad_endio_hook_pool:
         mempool_destroy(pool->mapping_pool);
 bad_mapping_pool:
+        dm_deferred_set_destroy(pool->all_io_ds);
+bad_all_io_ds:
+        dm_deferred_set_destroy(pool->shared_read_ds);
+bad_shared_read_ds:
         destroy_workqueue(pool->wq);
 bad_wq:
         dm_kcopyd_client_destroy(pool->copier);
 bad_kcopyd_client:
-        prison_destroy(pool->prison);
+        dm_bio_prison_destroy(pool->prison);
 bad_prison:
         kfree(pool);
 bad_pool:
@@ -2272,15 +1919,6 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
                 goto out_flags_changed;
         }
 
-        /*
-         * The block layer requires discard_granularity to be a power of 2.
-         */
-        if (pf.discard_enabled && !is_power_of_2(block_size)) {
-                ti->error = "Discard support must be disabled when the block size is not a power of 2";
-                r = -EINVAL;
-                goto out_flags_changed;
-        }
-
         pt->pool = pool;
         pt->ti = ti;
         pt->metadata_dev = metadata_dev;
@@ -2762,6 +2400,11 @@ static int pool_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
         return min(max_size, q->merge_bvec_fn(q, bvm, biovec));
 }
 
+static bool block_size_is_power_of_two(struct pool *pool)
+{
+        return pool->sectors_per_block_shift >= 0;
+}
+
 static void set_discard_limits(struct pool_c *pt, struct queue_limits *limits)
 {
         struct pool *pool = pt->pool;
@@ -2775,8 +2418,15 @@ static void set_discard_limits(struct pool_c *pt, struct queue_limits *limits)
         if (pt->adjusted_pf.discard_passdown) {
                 data_limits = &bdev_get_queue(pt->data_dev->bdev)->limits;
                 limits->discard_granularity = data_limits->discard_granularity;
-        } else
+        } else if (block_size_is_power_of_two(pool))
                 limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
+        else
+                /*
+                 * Use largest power of 2 that is a factor of sectors_per_block
+                 * but at least DATA_DEV_BLOCK_SIZE_MIN_SECTORS.
+                 */
+                limits->discard_granularity = max(1 << (ffs(pool->sectors_per_block) - 1),
+                                                  DATA_DEV_BLOCK_SIZE_MIN_SECTORS) << SECTOR_SHIFT;
 }
 
 static void pool_io_hints(struct dm_target *ti, struct queue_limits *limits)
@@ -2804,7 +2454,7 @@ static struct target_type pool_target = {
         .name = "thin-pool",
         .features = DM_TARGET_SINGLETON | DM_TARGET_ALWAYS_WRITEABLE |
                     DM_TARGET_IMMUTABLE,
-        .version = {1, 4, 0},
+        .version = {1, 5, 0},
         .module = THIS_MODULE,
         .ctr = pool_ctr,
         .dtr = pool_dtr,
@@ -2979,7 +2629,7 @@ static int thin_endio(struct dm_target *ti,
 
         if (h->shared_read_entry) {
                 INIT_LIST_HEAD(&work);
-                ds_dec(h->shared_read_entry, &work);
+                dm_deferred_entry_dec(h->shared_read_entry, &work);
 
                 spin_lock_irqsave(&pool->lock, flags);
                 list_for_each_entry_safe(m, tmp, &work, list) {
@@ -2992,7 +2642,7 @@ static int thin_endio(struct dm_target *ti,
 
         if (h->all_io_entry) {
                 INIT_LIST_HEAD(&work);
-                ds_dec(h->all_io_entry, &work);
+                dm_deferred_entry_dec(h->all_io_entry, &work);
                 spin_lock_irqsave(&pool->lock, flags);
                 list_for_each_entry_safe(m, tmp, &work, list)
                         list_add(&m->list, &pool->prepared_discards);
@@ -3095,7 +2745,7 @@ static void thin_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 static struct target_type thin_target = {
         .name = "thin",
-        .version = {1, 4, 0},
+        .version = {1, 5, 0},
         .module = THIS_MODULE,
         .ctr = thin_ctr,
         .dtr = thin_dtr,
@@ -3125,10 +2775,6 @@ static int __init dm_thin_init(void)
 
         r = -ENOMEM;
 
-        _cell_cache = KMEM_CACHE(dm_bio_prison_cell, 0);
-        if (!_cell_cache)
-                goto bad_cell_cache;
-
         _new_mapping_cache = KMEM_CACHE(dm_thin_new_mapping, 0);
         if (!_new_mapping_cache)
                 goto bad_new_mapping_cache;
@@ -3142,8 +2788,6 @@ static int __init dm_thin_init(void)
 bad_endio_hook_cache:
         kmem_cache_destroy(_new_mapping_cache);
 bad_new_mapping_cache:
-        kmem_cache_destroy(_cell_cache);
-bad_cell_cache:
         dm_unregister_target(&pool_target);
 bad_pool_target:
         dm_unregister_target(&thin_target);
@@ -3156,7 +2800,6 @@ static void dm_thin_exit(void)
         dm_unregister_target(&thin_target);
         dm_unregister_target(&pool_target);
 
-        kmem_cache_destroy(_cell_cache);
         kmem_cache_destroy(_new_mapping_cache);
         kmem_cache_destroy(_endio_hook_cache);
 }
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index 892ae2766aa6..9e7328bb4030 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -438,7 +438,7 @@ static void verity_prefetch_io(struct dm_verity *v, struct dm_verity_io *io)
                 verity_hash_at_level(v, io->block, i, &hash_block_start, NULL);
                 verity_hash_at_level(v, io->block + io->n_blocks - 1, i, &hash_block_end, NULL);
                 if (!i) {
-                        unsigned cluster = *(volatile unsigned *)&dm_verity_prefetch_cluster;
+                        unsigned cluster = ACCESS_ONCE(dm_verity_prefetch_cluster);
 
                         cluster >>= v->data_dev_block_bits;
                         if (unlikely(!cluster))
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 67ffa391edcf..02db9183ca01 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -71,6 +71,7 @@ struct dm_target_io {
         struct dm_io *io;
         struct dm_target *ti;
         union map_info info;
+        struct bio clone;
 };
 
 /*
@@ -86,12 +87,17 @@ struct dm_rq_target_io {
 };
 
 /*
- * For request-based dm.
- * One of these is allocated per bio.
+ * For request-based dm - the bio clones we allocate are embedded in these
+ * structs.
+ *
+ * We allocate these with bio_alloc_bioset, using the front_pad parameter when
+ * the bioset is created - this means the bio has to come at the end of the
+ * struct.
  */
 struct dm_rq_clone_bio_info {
         struct bio *orig;
         struct dm_rq_target_io *tio;
+        struct bio clone;
 };
 
 union map_info *dm_get_mapinfo(struct bio *bio)
@@ -209,8 +215,12 @@ struct dm_md_mempools {
 
 #define MIN_IOS 256
 static struct kmem_cache *_io_cache;
-static struct kmem_cache *_tio_cache;
 static struct kmem_cache *_rq_tio_cache;
+
+/*
+ * Unused now, and needs to be deleted. But since io_pool is overloaded and it's
+ * still used for _io_cache, I'm leaving this for a later cleanup
+ */
 static struct kmem_cache *_rq_bio_info_cache;
 
 static int __init local_init(void)
@@ -222,14 +232,9 @@ static int __init local_init(void)
         if (!_io_cache)
                 return r;
 
-        /* allocate a slab for the target ios */
-        _tio_cache = KMEM_CACHE(dm_target_io, 0);
-        if (!_tio_cache)
-                goto out_free_io_cache;
-
         _rq_tio_cache = KMEM_CACHE(dm_rq_target_io, 0);
         if (!_rq_tio_cache)
-                goto out_free_tio_cache;
+                goto out_free_io_cache;
 
         _rq_bio_info_cache = KMEM_CACHE(dm_rq_clone_bio_info, 0);
         if (!_rq_bio_info_cache)
@@ -255,8 +260,6 @@ out_free_rq_bio_info_cache:
         kmem_cache_destroy(_rq_bio_info_cache);
 out_free_rq_tio_cache:
         kmem_cache_destroy(_rq_tio_cache);
-out_free_tio_cache:
-        kmem_cache_destroy(_tio_cache);
 out_free_io_cache:
         kmem_cache_destroy(_io_cache);
 
@@ -267,7 +270,6 @@ static void local_exit(void)
 {
         kmem_cache_destroy(_rq_bio_info_cache);
         kmem_cache_destroy(_rq_tio_cache);
-        kmem_cache_destroy(_tio_cache);
         kmem_cache_destroy(_io_cache);
         unregister_blkdev(_major, _name);
         dm_uevent_exit();
@@ -453,7 +455,7 @@ static void free_io(struct mapped_device *md, struct dm_io *io)
 
 static void free_tio(struct mapped_device *md, struct dm_target_io *tio)
 {
-        mempool_free(tio, md->tio_pool);
+        bio_put(&tio->clone);
 }
 
 static struct dm_rq_target_io *alloc_rq_tio(struct mapped_device *md,
@@ -467,16 +469,6 @@ static void free_rq_tio(struct dm_rq_target_io *tio)
         mempool_free(tio, tio->md->tio_pool);
 }
 
-static struct dm_rq_clone_bio_info *alloc_bio_info(struct mapped_device *md)
-{
-        return mempool_alloc(md->io_pool, GFP_ATOMIC);
-}
-
-static void free_bio_info(struct dm_rq_clone_bio_info *info)
-{
-        mempool_free(info, info->tio->md->io_pool);
-}
-
 static int md_in_flight(struct mapped_device *md)
 {
         return atomic_read(&md->pending[READ]) +
@@ -681,13 +673,7 @@ static void clone_endio(struct bio *bio, int error)
                 }
         }
 
-        /*
-         * Store md for cleanup instead of tio which is about to get freed.
-         */
-        bio->bi_private = md->bs;
-
         free_tio(md, tio);
-        bio_put(bio);
         dec_pending(io, error);
 }
 
@@ -1007,12 +993,12 @@ int dm_set_target_max_io_len(struct dm_target *ti, sector_t len)
 }
 EXPORT_SYMBOL_GPL(dm_set_target_max_io_len);
 
-static void __map_bio(struct dm_target *ti, struct bio *clone,
-                      struct dm_target_io *tio)
+static void __map_bio(struct dm_target *ti, struct dm_target_io *tio)
 {
         int r;
         sector_t sector;
         struct mapped_device *md;
+        struct bio *clone = &tio->clone;
 
         clone->bi_end_io = clone_endio;
         clone->bi_private = tio;
@@ -1036,12 +1022,6 @@ static void __map_bio(struct dm_target *ti, struct bio *clone,
                 /* error the io and bail out, or requeue it if needed */
                 md = tio->io->md;
                 dec_pending(tio->io, r);
-                /*
-                 * Store bio_set for cleanup.
-                 */
-                clone->bi_end_io = NULL;
-                clone->bi_private = md->bs;
-                bio_put(clone);
                 free_tio(md, tio);
         } else if (r) {
                 DMWARN("unimplemented target map return value: %d", r);
@@ -1059,25 +1039,16 @@ struct clone_info {
         unsigned short idx;
 };
 
-static void dm_bio_destructor(struct bio *bio)
-{
-        struct bio_set *bs = bio->bi_private;
-
-        bio_free(bio, bs);
-}
-
 /*
  * Creates a little bio that just does part of a bvec.
  */
-static struct bio *split_bvec(struct bio *bio, sector_t sector,
-                              unsigned short idx, unsigned int offset,
-                              unsigned int len, struct bio_set *bs)
+static void split_bvec(struct dm_target_io *tio, struct bio *bio,
+                       sector_t sector, unsigned short idx, unsigned int offset,
+                       unsigned int len, struct bio_set *bs)
 {
-        struct bio *clone;
+        struct bio *clone = &tio->clone;
         struct bio_vec *bv = bio->bi_io_vec + idx;
 
-        clone = bio_alloc_bioset(GFP_NOIO, 1, bs);
-        clone->bi_destructor = dm_bio_destructor;
         *clone->bi_io_vec = *bv;
 
         clone->bi_sector = sector;
@@ -1090,26 +1061,23 @@ static struct bio *split_bvec(struct bio *bio, sector_t sector,
         clone->bi_flags |= 1 << BIO_CLONED;
 
         if (bio_integrity(bio)) {
-                bio_integrity_clone(clone, bio, GFP_NOIO, bs);
+                bio_integrity_clone(clone, bio, GFP_NOIO);
                 bio_integrity_trim(clone,
                                    bio_sector_offset(bio, idx, offset), len);
         }
-
-        return clone;
 }
 
 /*
  * Creates a bio that consists of range of complete bvecs.
  */
-static struct bio *clone_bio(struct bio *bio, sector_t sector,
-                             unsigned short idx, unsigned short bv_count,
-                             unsigned int len, struct bio_set *bs)
+static void clone_bio(struct dm_target_io *tio, struct bio *bio,
+                      sector_t sector, unsigned short idx,
+                      unsigned short bv_count, unsigned int len,
+                      struct bio_set *bs)
 {
-        struct bio *clone;
+        struct bio *clone = &tio->clone;
 
-        clone = bio_alloc_bioset(GFP_NOIO, bio->bi_max_vecs, bs);
         __bio_clone(clone, bio);
-        clone->bi_destructor = dm_bio_destructor;
         clone->bi_sector = sector;
         clone->bi_idx = idx;
         clone->bi_vcnt = idx + bv_count;
@@ -1117,20 +1085,22 @@ static struct bio *clone_bio(struct bio *bio, sector_t sector,
         clone->bi_flags &= ~(1 << BIO_SEG_VALID);
 
         if (bio_integrity(bio)) {
-                bio_integrity_clone(clone, bio, GFP_NOIO, bs);
+                bio_integrity_clone(clone, bio, GFP_NOIO);
 
                 if (idx != bio->bi_idx || clone->bi_size < bio->bi_size)
                         bio_integrity_trim(clone,
                                            bio_sector_offset(bio, idx, 0), len);
         }
-
-        return clone;
 }
 
 static struct dm_target_io *alloc_tio(struct clone_info *ci,
-                                      struct dm_target *ti)
+                                      struct dm_target *ti, int nr_iovecs)
 {
-        struct dm_target_io *tio = mempool_alloc(ci->md->tio_pool, GFP_NOIO);
+        struct dm_target_io *tio;
+        struct bio *clone;
+
+        clone = bio_alloc_bioset(GFP_NOIO, nr_iovecs, ci->md->bs);
+        tio = container_of(clone, struct dm_target_io, clone);
 
         tio->io = ci->io;
         tio->ti = ti;
@@ -1142,8 +1112,8 @@ static struct dm_target_io *alloc_tio(struct clone_info *ci,
 static void __issue_target_request(struct clone_info *ci, struct dm_target *ti,
                                    unsigned request_nr, sector_t len)
 {
-        struct dm_target_io *tio = alloc_tio(ci, ti);
-        struct bio *clone;
+        struct dm_target_io *tio = alloc_tio(ci, ti, ci->bio->bi_max_vecs);
+        struct bio *clone = &tio->clone;
 
         tio->info.target_request_nr = request_nr;
 
@@ -1152,15 +1122,14 @@ static void __issue_target_request(struct clone_info *ci, struct dm_target *ti,
          * ci->bio->bi_max_vecs is BIO_INLINE_VECS anyway, for both flush
          * and discard, so no need for concern about wasted bvec allocations.
          */
-        clone = bio_alloc_bioset(GFP_NOIO, ci->bio->bi_max_vecs, ci->md->bs);
-        __bio_clone(clone, ci->bio);
-        clone->bi_destructor = dm_bio_destructor;
+
+         __bio_clone(clone, ci->bio);
         if (len) {
                 clone->bi_sector = ci->sector;
                 clone->bi_size = to_bytes(len);
         }
 
-        __map_bio(ti, clone, tio);
+        __map_bio(ti, tio);
 }
 
 static void __issue_target_requests(struct clone_info *ci, struct dm_target *ti,
@@ -1189,14 +1158,13 @@ static int __clone_and_map_empty_flush(struct clone_info *ci)
  */
 static void __clone_and_map_simple(struct clone_info *ci, struct dm_target *ti)
 {
-        struct bio *clone, *bio = ci->bio;
+        struct bio *bio = ci->bio;
         struct dm_target_io *tio;
 
-        tio = alloc_tio(ci, ti);
-        clone = clone_bio(bio, ci->sector, ci->idx,
-                          bio->bi_vcnt - ci->idx, ci->sector_count,
-                          ci->md->bs);
-        __map_bio(ti, clone, tio);
+        tio = alloc_tio(ci, ti, bio->bi_max_vecs);
+        clone_bio(tio, bio, ci->sector, ci->idx, bio->bi_vcnt - ci->idx,
+                  ci->sector_count, ci->md->bs);
+        __map_bio(ti, tio);
         ci->sector_count = 0;
 }
 
@@ -1234,7 +1202,7 @@ static int __clone_and_map_discard(struct clone_info *ci)
 
 static int __clone_and_map(struct clone_info *ci)
 {
-        struct bio *clone, *bio = ci->bio;
+        struct bio *bio = ci->bio;
         struct dm_target *ti;
         sector_t len = 0, max;
         struct dm_target_io *tio;
@@ -1274,10 +1242,10 @@ static int __clone_and_map(struct clone_info *ci)
                         len += bv_len;
                 }
 
-                tio = alloc_tio(ci, ti);
-                clone = clone_bio(bio, ci->sector, ci->idx, i - ci->idx, len,
-                                  ci->md->bs);
-                __map_bio(ti, clone, tio);
+                tio = alloc_tio(ci, ti, bio->bi_max_vecs);
+                clone_bio(tio, bio, ci->sector, ci->idx, i - ci->idx, len,
+                          ci->md->bs);
+                __map_bio(ti, tio);
 
                 ci->sector += len;
                 ci->sector_count -= len;
@@ -1302,12 +1270,11 @@ static int __clone_and_map(struct clone_info *ci)
 
                         len = min(remaining, max);
 
-                        tio = alloc_tio(ci, ti);
-                        clone = split_bvec(bio, ci->sector, ci->idx,
-                                           bv->bv_offset + offset, len,
-                                           ci->md->bs);
+                        tio = alloc_tio(ci, ti, 1);
+                        split_bvec(tio, bio, ci->sector, ci->idx,
+                                   bv->bv_offset + offset, len, ci->md->bs);
 
-                        __map_bio(ti, clone, tio);
+                        __map_bio(ti, tio);
 
                         ci->sector += len;
                         ci->sector_count -= len;
@@ -1484,30 +1451,17 @@ void dm_dispatch_request(struct request *rq)
 }
 EXPORT_SYMBOL_GPL(dm_dispatch_request);
 
-static void dm_rq_bio_destructor(struct bio *bio)
-{
-        struct dm_rq_clone_bio_info *info = bio->bi_private;
-        struct mapped_device *md = info->tio->md;
-
-        free_bio_info(info);
-        bio_free(bio, md->bs);
-}
-
 static int dm_rq_bio_constructor(struct bio *bio, struct bio *bio_orig,
                                  void *data)
 {
         struct dm_rq_target_io *tio = data;
-        struct mapped_device *md = tio->md;
-        struct dm_rq_clone_bio_info *info = alloc_bio_info(md);
-
-        if (!info)
-                return -ENOMEM;
+        struct dm_rq_clone_bio_info *info =
+                container_of(bio, struct dm_rq_clone_bio_info, clone);
 
         info->orig = bio_orig;
         info->tio = tio;
         bio->bi_end_io = end_clone_bio;
         bio->bi_private = info;
-        bio->bi_destructor = dm_rq_bio_destructor;
 
         return 0;
 }
@@ -1988,7 +1942,7 @@ static void __bind_mempools(struct mapped_device *md, struct dm_table *t)
 {
         struct dm_md_mempools *p;
 
-        if (md->io_pool && md->tio_pool && md->bs)
+        if (md->io_pool && (md->tio_pool || dm_table_get_type(t) == DM_TYPE_BIO_BASED) && md->bs)
                 /* the md already has necessary mempools */
                 goto out;
 
@@ -2765,13 +2719,18 @@ struct dm_md_mempools *dm_alloc_md_mempools(unsigned type, unsigned integrity)
         if (!pools->io_pool)
                 goto free_pools_and_out;
 
-        pools->tio_pool = (type == DM_TYPE_BIO_BASED) ?
-                          mempool_create_slab_pool(MIN_IOS, _tio_cache) :
-                          mempool_create_slab_pool(MIN_IOS, _rq_tio_cache);
-        if (!pools->tio_pool)
-                goto free_io_pool_and_out;
+        pools->tio_pool = NULL;
+        if (type == DM_TYPE_REQUEST_BASED) {
+                pools->tio_pool = mempool_create_slab_pool(MIN_IOS, _rq_tio_cache);
+                if (!pools->tio_pool)
+                        goto free_io_pool_and_out;
+        }
 
-        pools->bs = bioset_create(pool_size, 0);
+        pools->bs = (type == DM_TYPE_BIO_BASED) ?
+                bioset_create(pool_size,
+                              offsetof(struct dm_target_io, clone)) :
+                bioset_create(pool_size,
+                              offsetof(struct dm_rq_clone_bio_info, clone));
         if (!pools->bs)
                 goto free_tio_pool_and_out;
 
@@ -2784,7 +2743,8 @@ free_bioset_and_out:
         bioset_free(pools->bs);
 
 free_tio_pool_and_out:
-        mempool_destroy(pools->tio_pool);
+        if (pools->tio_pool)
+                mempool_destroy(pools->tio_pool);
 
 free_io_pool_and_out:
         mempool_destroy(pools->io_pool);
diff --git a/drivers/md/faulty.c b/drivers/md/faulty.c
index 45135f69509c..5e7dc772f5de 100644
--- a/drivers/md/faulty.c
+++ b/drivers/md/faulty.c
@@ -315,8 +315,11 @@ static int run(struct mddev *mddev)
         }
         conf->nfaults = 0;
 
-        rdev_for_each(rdev, mddev)
+        rdev_for_each(rdev, mddev) {
                 conf->rdev = rdev;
+                disk_stack_limits(mddev->gendisk, rdev->bdev,
+                                  rdev->data_offset << 9);
+        }
 
         md_set_array_sectors(mddev, faulty_size(mddev, 0, 0));
         mddev->private = conf;
diff --git a/drivers/md/linear.c b/drivers/md/linear.c
index fa211d80fc0a..21014836bdbf 100644
--- a/drivers/md/linear.c
+++ b/drivers/md/linear.c
@@ -138,6 +138,7 @@ static struct linear_conf *linear_conf(struct mddev *mddev, int raid_disks)
         struct linear_conf *conf;
         struct md_rdev *rdev;
         int i, cnt;
+        bool discard_supported = false;
 
         conf = kzalloc (sizeof (*conf) + raid_disks*sizeof(struct dev_info),
                         GFP_KERNEL);
@@ -171,6 +172,8 @@ static struct linear_conf *linear_conf(struct mddev *mddev, int raid_disks)
                 conf->array_sectors += rdev->sectors;
                 cnt++;
 
+                if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+                        discard_supported = true;
         }
         if (cnt != raid_disks) {
                 printk(KERN_ERR "md/linear:%s: not enough drives present. Aborting!\n",
@@ -178,6 +181,11 @@ static struct linear_conf *linear_conf(struct mddev *mddev, int raid_disks)
                 goto out;
         }
 
+        if (!discard_supported)
+                queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+        else
+                queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
         /*
          * Here we calculate the device offsets.
          */
@@ -244,7 +252,9 @@ static int linear_add(struct mddev *mddev, struct md_rdev *rdev)
         if (!newconf)
                 return -ENOMEM;
 
-        oldconf = rcu_dereference(mddev->private);
+        oldconf = rcu_dereference_protected(mddev->private,
+                                            lockdep_is_held(
+                                                    &mddev->reconfig_mutex));
         mddev->raid_disks++;
         rcu_assign_pointer(mddev->private, newconf);
         md_set_array_sectors(mddev, linear_size(mddev, 0, 0));
@@ -256,7 +266,10 @@ static int linear_add(struct mddev *mddev, struct md_rdev *rdev)
 
 static int linear_stop (struct mddev *mddev)
 {
-        struct linear_conf *conf = mddev->private;
+        struct linear_conf *conf =
+                rcu_dereference_protected(mddev->private,
+                                          lockdep_is_held(
+                                                  &mddev->reconfig_mutex));
 
         /*
          * We do not require rcu protection here since
@@ -326,6 +339,14 @@ static void linear_make_request(struct mddev *mddev, struct bio *bio)
         bio->bi_sector = bio->bi_sector - start_sector
                 + tmp_dev->rdev->data_offset;
         rcu_read_unlock();
+
+        if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+                     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
+                /* Just ignore it */
+                bio_endio(bio, 0);
+                return;
+        }
+
         generic_make_request(bio);
 }
 
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 308e87b417e0..9ab768acfb62 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -155,32 +155,17 @@ static int start_readonly;
  * like bio_clone, but with a local bio set
  */
 
-static void mddev_bio_destructor(struct bio *bio)
-{
-        struct mddev *mddev, **mddevp;
-
-        mddevp = (void*)bio;
-        mddev = mddevp[-1];
-
-        bio_free(bio, mddev->bio_set);
-}
-
 struct bio *bio_alloc_mddev(gfp_t gfp_mask, int nr_iovecs,
                             struct mddev *mddev)
 {
         struct bio *b;
-        struct mddev **mddevp;
 
         if (!mddev || !mddev->bio_set)
                 return bio_alloc(gfp_mask, nr_iovecs);
 
-        b = bio_alloc_bioset(gfp_mask, nr_iovecs,
-                             mddev->bio_set);
+        b = bio_alloc_bioset(gfp_mask, nr_iovecs, mddev->bio_set);
         if (!b)
                 return NULL;
-        mddevp = (void*)b;
-        mddevp[-1] = mddev;
-        b->bi_destructor = mddev_bio_destructor;
         return b;
 }
 EXPORT_SYMBOL_GPL(bio_alloc_mddev);
@@ -188,32 +173,10 @@ EXPORT_SYMBOL_GPL(bio_alloc_mddev);
 struct bio *bio_clone_mddev(struct bio *bio, gfp_t gfp_mask,
                             struct mddev *mddev)
 {
-        struct bio *b;
-        struct mddev **mddevp;
-
         if (!mddev || !mddev->bio_set)
                 return bio_clone(bio, gfp_mask);
 
-        b = bio_alloc_bioset(gfp_mask, bio->bi_max_vecs,
-                             mddev->bio_set);
-        if (!b)
-                return NULL;
-        mddevp = (void*)b;
-        mddevp[-1] = mddev;
-        b->bi_destructor = mddev_bio_destructor;
-        __bio_clone(b, bio);
-        if (bio_integrity(bio)) {
-                int ret;
-
-                ret = bio_integrity_clone(b, bio, gfp_mask, mddev->bio_set);
-
-                if (ret < 0) {
-                        bio_put(b);
-                        return NULL;
-                }
-        }
-
-        return b;
+        return bio_clone_bioset(bio, gfp_mask, mddev->bio_set);
 }
 EXPORT_SYMBOL_GPL(bio_clone_mddev);
 
@@ -711,7 +674,18 @@ static struct md_rdev * find_rdev_nr(struct mddev *mddev, int nr)
         return NULL;
 }
 
-static struct md_rdev * find_rdev(struct mddev * mddev, dev_t dev)
+static struct md_rdev *find_rdev_nr_rcu(struct mddev *mddev, int nr)
+{
+        struct md_rdev *rdev;
+
+        rdev_for_each_rcu(rdev, mddev)
+                if (rdev->desc_nr == nr)
+                        return rdev;
+
+        return NULL;
+}
+
+static struct md_rdev *find_rdev(struct mddev *mddev, dev_t dev)
 {
         struct md_rdev *rdev;
 
@@ -722,6 +696,17 @@ static struct md_rdev * find_rdev(struct mddev * mddev, dev_t dev)
         return NULL;
 }
 
+static struct md_rdev *find_rdev_rcu(struct mddev *mddev, dev_t dev)
+{
+        struct md_rdev *rdev;
+
+        rdev_for_each_rcu(rdev, mddev)
+                if (rdev->bdev->bd_dev == dev)
+                        return rdev;
+
+        return NULL;
+}
+
 static struct md_personality *find_pers(int level, char *clevel)
 {
         struct md_personality *pers;
@@ -2059,8 +2044,14 @@ EXPORT_SYMBOL(md_integrity_register);
 /* Disable data integrity if non-capable/non-matching disk is being added */
 void md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev)
 {
-        struct blk_integrity *bi_rdev = bdev_get_integrity(rdev->bdev);
-        struct blk_integrity *bi_mddev = blk_get_integrity(mddev->gendisk);
+        struct blk_integrity *bi_rdev;
+        struct blk_integrity *bi_mddev;
+
+        if (!mddev->gendisk)
+                return;
+
+        bi_rdev = bdev_get_integrity(rdev->bdev);
+        bi_mddev = blk_get_integrity(mddev->gendisk);
 
         if (!bi_mddev) /* nothing to do */
                 return;
@@ -3791,6 +3782,8 @@ resync_start_store(struct mddev *mddev, const char *buf, size_t len)
                 return -EINVAL;
 
         mddev->recovery_cp = n;
+        if (mddev->pers)
+                set_bit(MD_CHANGE_CLEAN, &mddev->flags);
         return len;
 }
 static struct md_sysfs_entry md_resync_start =
@@ -4268,6 +4261,13 @@ action_store(struct mddev *mddev, const char *page, size_t len)
                 set_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
                 set_bit(MD_RECOVERY_SYNC, &mddev->recovery);
         }
+        if (mddev->ro == 2) {
+                /* A write to sync_action is enough to justify
+                 * canceling read-auto mode
+                 */
+                mddev->ro = 0;
+                md_wakeup_thread(mddev->sync_thread);
+        }
         set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
         md_wakeup_thread(mddev->thread);
         sysfs_notify_dirent_safe(mddev->sysfs_action);
@@ -4278,7 +4278,8 @@ static ssize_t
 mismatch_cnt_show(struct mddev *mddev, char *page)
 {
         return sprintf(page, "%llu\n",
-                       (unsigned long long) mddev->resync_mismatches);
+                       (unsigned long long)
+                       atomic64_read(&mddev->resync_mismatches));
 }
 
 static struct md_sysfs_entry md_scan_mode =
@@ -4399,6 +4400,10 @@ sync_completed_show(struct mddev *mddev, char *page)
         if (!test_bit(MD_RECOVERY_RUNNING, &mddev->recovery))
                 return sprintf(page, "none\n");
 
+        if (mddev->curr_resync == 1 ||
+            mddev->curr_resync == 2)
+                return sprintf(page, "delayed\n");
+
         if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
             test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
                 max_sectors = mddev->resync_max_sectors;
@@ -5006,8 +5011,7 @@ int md_run(struct mddev *mddev)
         }
 
         if (mddev->bio_set == NULL)
-                mddev->bio_set = bioset_create(BIO_POOL_SIZE,
-                                               sizeof(struct mddev *));
+                mddev->bio_set = bioset_create(BIO_POOL_SIZE, 0);
 
         spin_lock(&pers_lock);
         pers = find_pers(mddev->level, mddev->clevel);
@@ -5245,7 +5249,7 @@ static void md_clean(struct mddev *mddev)
         mddev->new_layout = 0;
         mddev->new_chunk_sectors = 0;
         mddev->curr_resync = 0;
-        mddev->resync_mismatches = 0;
+        atomic64_set(&mddev->resync_mismatches, 0);
         mddev->suspend_lo = mddev->suspend_hi = 0;
         mddev->sync_speed_min = mddev->sync_speed_max = 0;
         mddev->recovery = 0;
@@ -5547,8 +5551,9 @@ static int get_array_info(struct mddev * mddev, void __user * arg)
         int nr,working,insync,failed,spare;
         struct md_rdev *rdev;
 
-        nr=working=insync=failed=spare=0;
-        rdev_for_each(rdev, mddev) {
+        nr = working = insync = failed = spare = 0;
+        rcu_read_lock();
+        rdev_for_each_rcu(rdev, mddev) {
                 nr++;
                 if (test_bit(Faulty, &rdev->flags))
                         failed++;
@@ -5560,6 +5565,7 @@ static int get_array_info(struct mddev * mddev, void __user * arg)
                                 spare++;
                 }
         }
+        rcu_read_unlock();
 
         info.major_version = mddev->major_version;
         info.minor_version = mddev->minor_version;
@@ -5643,7 +5649,8 @@ static int get_disk_info(struct mddev * mddev, void __user * arg)
         if (copy_from_user(&info, arg, sizeof(info)))
                 return -EFAULT;
 
-        rdev = find_rdev_nr(mddev, info.number);
+        rcu_read_lock();
+        rdev = find_rdev_nr_rcu(mddev, info.number);
         if (rdev) {
                 info.major = MAJOR(rdev->bdev->bd_dev);
                 info.minor = MINOR(rdev->bdev->bd_dev);
@@ -5662,6 +5669,7 @@ static int get_disk_info(struct mddev * mddev, void __user * arg)
                 info.raid_disk = -1;
                 info.state = (1<<MD_DISK_REMOVED);
         }
+        rcu_read_unlock();
 
         if (copy_to_user(arg, &info, sizeof(info)))
                 return -EFAULT;
@@ -6270,18 +6278,22 @@ static int update_array_info(struct mddev *mddev, mdu_array_info_t *info)
 static int set_disk_faulty(struct mddev *mddev, dev_t dev)
 {
         struct md_rdev *rdev;
+        int err = 0;
 
         if (mddev->pers == NULL)
                 return -ENODEV;
 
-        rdev = find_rdev(mddev, dev);
+        rcu_read_lock();
+        rdev = find_rdev_rcu(mddev, dev);
         if (!rdev)
-                return -ENODEV;
-
-        md_error(mddev, rdev);
-        if (!test_bit(Faulty, &rdev->flags))
-                return -EBUSY;
-        return 0;
+                err =  -ENODEV;
+        else {
+                md_error(mddev, rdev);
+                if (!test_bit(Faulty, &rdev->flags))
+                        err = -EBUSY;
+        }
+        rcu_read_unlock();
+        return err;
 }
 
 /*
@@ -6353,6 +6365,27 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
                 goto abort;
         }
 
+        /* Some actions do not requires the mutex */
+        switch (cmd) {
+        case GET_ARRAY_INFO:
+                if (!mddev->raid_disks && !mddev->external)
+                        err = -ENODEV;
+                else
+                        err = get_array_info(mddev, argp);
+                goto abort;
+
+        case GET_DISK_INFO:
+                if (!mddev->raid_disks && !mddev->external)
+                        err = -ENODEV;
+                else
+                        err = get_disk_info(mddev, argp);
+                goto abort;
+
+        case SET_DISK_FAULTY:
+                err = set_disk_faulty(mddev, new_decode_dev(arg));
+                goto abort;
+        }
+
         err = mddev_lock(mddev);
         if (err) {
                 printk(KERN_INFO 
@@ -6425,18 +6458,10 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
          */
         switch (cmd)
         {
-                case GET_ARRAY_INFO:
-                        err = get_array_info(mddev, argp);
-                        goto done_unlock;
-
                 case GET_BITMAP_FILE:
                         err = get_bitmap_file(mddev, argp);
                         goto done_unlock;
 
-                case GET_DISK_INFO:
-                        err = get_disk_info(mddev, argp);
-                        goto done_unlock;
-
                 case RESTART_ARRAY_RW:
                         err = restart_array(mddev);
                         goto done_unlock;
@@ -6518,10 +6543,6 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
                         err = hot_add_disk(mddev, new_decode_dev(arg));
                         goto done_unlock;
 
-                case SET_DISK_FAULTY:
-                        err = set_disk_faulty(mddev, new_decode_dev(arg));
-                        goto done_unlock;
-
                 case RUN_ARRAY:
                         err = do_md_run(mddev);
                         goto done_unlock;
@@ -6679,7 +6700,7 @@ static int md_thread(void * arg)
 
                 clear_bit(THREAD_WAKEUP, &thread->flags);
                 if (!kthread_should_stop())
-                        thread->run(thread->mddev);
+                        thread->run(thread);
         }
 
         return 0;
@@ -6694,8 +6715,8 @@ void md_wakeup_thread(struct md_thread *thread)
         }
 }
 
-struct md_thread *md_register_thread(void (*run) (struct mddev *), struct mddev *mddev,
-                                 const char *name)
+struct md_thread *md_register_thread(void (*run) (struct md_thread *),
+                struct mddev *mddev, const char *name)
 {
         struct md_thread *thread;
 
@@ -6790,7 +6811,11 @@ static void status_resync(struct seq_file *seq, struct mddev * mddev)
         int scale;
         unsigned int per_milli;
 
-        resync = mddev->curr_resync - atomic_read(&mddev->recovery_active);
+        if (mddev->curr_resync <= 3)
+                resync = 0;
+        else
+                resync = mddev->curr_resync
+                        - atomic_read(&mddev->recovery_active);
 
         if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
             test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
@@ -7016,7 +7041,7 @@ static int md_seq_show(struct seq_file *seq, void *v)
                                 if (mddev->curr_resync > 2) {
                                         status_resync(seq, mddev);
                                         seq_printf(seq, "\n      ");
-                                } else if (mddev->curr_resync == 1 || mddev->curr_resync == 2)
+                                } else if (mddev->curr_resync >= 1)
                                         seq_printf(seq, "\tresync=DELAYED\n      ");
                                 else if (mddev->recovery_cp < MaxSector)
                                         seq_printf(seq, "\tresync=PENDING\n      ");
@@ -7244,8 +7269,9 @@ EXPORT_SYMBOL_GPL(md_allow_write);
 
 #define SYNC_MARKS      10
 #define SYNC_MARK_STEP  (3*HZ)
-void md_do_sync(struct mddev *mddev)
+void md_do_sync(struct md_thread *thread)
 {
+        struct mddev *mddev = thread->mddev;
         struct mddev *mddev2;
         unsigned int currspeed = 0,
                  window;
@@ -7349,7 +7375,7 @@ void md_do_sync(struct mddev *mddev)
                  * which defaults to physical size, but can be virtual size
                  */
                 max_sectors = mddev->resync_max_sectors;
-                mddev->resync_mismatches = 0;
+                atomic64_set(&mddev->resync_mismatches, 0);
                 /* we don't use the checkpoint if there's a bitmap */
                 if (test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery))
                         j = mddev->resync_min;
@@ -7405,8 +7431,11 @@ void md_do_sync(struct mddev *mddev)
                        "md: resuming %s of %s from checkpoint.\n",
                        desc, mdname(mddev));
                 mddev->curr_resync = j;
-        }
+        } else
+                mddev->curr_resync = 3; /* no longer delayed */
         mddev->curr_resync_completed = j;
+        sysfs_notify(&mddev->kobj, NULL, "sync_completed");
+        md_new_event(mddev);
 
         blk_start_plug(&plug);
         while (j < max_sectors) {
@@ -7459,7 +7488,8 @@ void md_do_sync(struct mddev *mddev)
                         break;
 
                 j += sectors;
-                if (j>1) mddev->curr_resync = j;
+                if (j > 2)
+                        mddev->curr_resync = j;
                 mddev->curr_mark_cnt = io_sectors;
                 if (last_check == 0)
                         /* this is the earliest that rebuild will be
@@ -7581,8 +7611,6 @@ static int remove_and_add_spares(struct mddev *mddev)
         int spares = 0;
         int removed = 0;
 
-        mddev->curr_resync_completed = 0;
-
         rdev_for_each(rdev, mddev)
                 if (rdev->raid_disk >= 0 &&
                     !test_bit(Blocked, &rdev->flags) &&
@@ -7777,6 +7805,7 @@ void md_check_recovery(struct mddev *mddev)
                 /* Set RUNNING before clearing NEEDED to avoid
                  * any transients in the value of "sync_action".
                  */
+                mddev->curr_resync_completed = 0;
                 set_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
                 /* Clear some bits that don't mean anything, but
                  * might be left set
@@ -7790,7 +7819,7 @@ void md_check_recovery(struct mddev *mddev)
                 /* no recovery is running.
                  * remove any failed drives, then
                  * add spares if possible.
-                 * Spare are also removed and re-added, to allow
+                 * Spares are also removed and re-added, to allow
                  * the personality to fail the re-add.
                  */
 
diff --git a/drivers/md/md.h b/drivers/md/md.h
index f385b038589d..af443ab868db 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -282,7 +282,7 @@ struct mddev {
 
         sector_t                        resync_max_sectors; /* may be set by personality */
 
-        sector_t                        resync_mismatches; /* count of sectors where
+        atomic64_t                      resync_mismatches; /* count of sectors where
                                                             * parity/replica mismatch found
                                                             */
 
@@ -540,12 +540,13 @@ static inline void sysfs_unlink_rdev(struct mddev *mddev, struct md_rdev *rdev)
         list_for_each_entry_rcu(rdev, &((mddev)->disks), same_set)
 
 struct md_thread {
-        void                    (*run) (struct mddev *mddev);
+        void                    (*run) (struct md_thread *thread);
         struct mddev            *mddev;
         wait_queue_head_t       wqueue;
         unsigned long           flags;
         struct task_struct      *tsk;
         unsigned long           timeout;
+        void                    *private;
 };
 
 #define THREAD_WAKEUP  0
@@ -584,7 +585,7 @@ static inline void safe_put_page(struct page *p)
 extern int register_md_personality(struct md_personality *p);
 extern int unregister_md_personality(struct md_personality *p);
 extern struct md_thread *md_register_thread(
-        void (*run)(struct mddev *mddev),
+        void (*run)(struct md_thread *thread),
         struct mddev *mddev,
         const char *name);
 extern void md_unregister_thread(struct md_thread **threadp);
@@ -603,7 +604,7 @@ extern void md_super_write(struct mddev *mddev, struct md_rdev *rdev,
 extern void md_super_wait(struct mddev *mddev);
 extern int sync_page_io(struct md_rdev *rdev, sector_t sector, int size, 
                         struct page *page, int rw, bool metadata_op);
-extern void md_do_sync(struct mddev *mddev);
+extern void md_do_sync(struct md_thread *thread);
 extern void md_new_event(struct mddev *mddev);
 extern int md_allow_write(struct mddev *mddev);
 extern void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev);
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index 61a1833ebaf3..1642eae75a33 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -335,8 +335,9 @@ abort:
  *      3.      Performs writes following reads for array syncronising.
  */
 
-static void multipathd (struct mddev *mddev)
+static void multipathd(struct md_thread *thread)
 {
+        struct mddev *mddev = thread->mddev;
         struct multipath_bh *mp_bh;
         struct bio *bio;
         unsigned long flags;
diff --git a/drivers/md/persistent-data/dm-space-map-common.c b/drivers/md/persistent-data/dm-space-map-common.c
index d77602d63c83..f3a9af8cdec3 100644
--- a/drivers/md/persistent-data/dm-space-map-common.c
+++ b/drivers/md/persistent-data/dm-space-map-common.c
@@ -434,14 +434,14 @@ int sm_ll_insert(struct ll_disk *ll, dm_block_t b,
         if (ref_count && !old) {
                 *ev = SM_ALLOC;
                 ll->nr_allocated++;
-                ie_disk.nr_free = cpu_to_le32(le32_to_cpu(ie_disk.nr_free) - 1);
+                le32_add_cpu(&ie_disk.nr_free, -1);
                 if (le32_to_cpu(ie_disk.none_free_before) == bit)
                         ie_disk.none_free_before = cpu_to_le32(bit + 1);
 
         } else if (old && !ref_count) {
                 *ev = SM_FREE;
                 ll->nr_allocated--;
-                ie_disk.nr_free = cpu_to_le32(le32_to_cpu(ie_disk.nr_free) + 1);
+                le32_add_cpu(&ie_disk.nr_free, 1);
                 ie_disk.none_free_before = cpu_to_le32(min(le32_to_cpu(ie_disk.none_free_before), bit));
         }
 
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index de63a1fc3737..24b359717a7e 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -88,6 +88,7 @@ static int create_strip_zones(struct mddev *mddev, struct r0conf **private_conf)
         char b[BDEVNAME_SIZE];
         char b2[BDEVNAME_SIZE];
         struct r0conf *conf = kzalloc(sizeof(*conf), GFP_KERNEL);
+        bool discard_supported = false;
 
         if (!conf)
                 return -ENOMEM;
@@ -195,6 +196,9 @@ static int create_strip_zones(struct mddev *mddev, struct r0conf **private_conf)
                 if (!smallest || (rdev1->sectors < smallest->sectors))
                         smallest = rdev1;
                 cnt++;
+
+                if (blk_queue_discard(bdev_get_queue(rdev1->bdev)))
+                        discard_supported = true;
         }
         if (cnt != mddev->raid_disks) {
                 printk(KERN_ERR "md/raid0:%s: too few disks (%d of %d) - "
@@ -272,6 +276,11 @@ static int create_strip_zones(struct mddev *mddev, struct r0conf **private_conf)
         blk_queue_io_opt(mddev->queue,
                          (mddev->chunk_sectors << 9) * mddev->raid_disks);
 
+        if (!discard_supported)
+                queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+        else
+                queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
         pr_debug("md/raid0:%s: done.\n", mdname(mddev));
         *private_conf = conf;
 
@@ -422,6 +431,8 @@ static int raid0_run(struct mddev *mddev)
         if (md_check_no_bitmap(mddev))
                 return -EINVAL;
         blk_queue_max_hw_sectors(mddev->queue, mddev->chunk_sectors);
+        blk_queue_max_write_same_sectors(mddev->queue, mddev->chunk_sectors);
+        blk_queue_max_discard_sectors(mddev->queue, mddev->chunk_sectors);
 
         /* if private is not null, we are here after takeover */
         if (mddev->private == NULL) {
@@ -509,7 +520,7 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
                 sector_t sector = bio->bi_sector;
                 struct bio_pair *bp;
                 /* Sanity check -- queue functions should prevent this happening */
-                if (bio->bi_vcnt != 1 ||
+                if ((bio->bi_vcnt != 1 && bio->bi_vcnt != 0) ||
                     bio->bi_idx != 0)
                         goto bad_map;
                 /* This is a one page bio that upper layers
@@ -535,6 +546,13 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
         bio->bi_sector = sector_offset + zone->dev_start +
                 tmp_dev->data_offset;
 
+        if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+                     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
+                /* Just ignore it */
+                bio_endio(bio, 0);
+                return;
+        }
+
         generic_make_request(bio);
         return;
 
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 611b5f797618..636bae0405e8 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -333,9 +333,10 @@ static void raid1_end_read_request(struct bio *bio, int error)
                 spin_unlock_irqrestore(&conf->device_lock, flags);
         }
 
-        if (uptodate)
+        if (uptodate) {
                 raid_end_bio_io(r1_bio);
-        else {
+                rdev_dec_pending(conf->mirrors[mirror].rdev, conf->mddev);
+        } else {
                 /*
                  * oops, read error:
                  */
@@ -349,9 +350,8 @@ static void raid1_end_read_request(struct bio *bio, int error)
                         (unsigned long long)r1_bio->sector);
                 set_bit(R1BIO_ReadError, &r1_bio->state);
                 reschedule_retry(r1_bio);
+                /* don't drop the reference on read_disk yet */
         }
-
-        rdev_dec_pending(conf->mirrors[mirror].rdev, conf->mddev);
 }
 
 static void close_write(struct r1bio *r1_bio)
@@ -781,7 +781,12 @@ static void flush_pending_writes(struct r1conf *conf)
                 while (bio) { /* submit pending writes */
                         struct bio *next = bio->bi_next;
                         bio->bi_next = NULL;
-                        generic_make_request(bio);
+                        if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+                            !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
+                                /* Just ignore it */
+                                bio_endio(bio, 0);
+                        else
+                                generic_make_request(bio);
                         bio = next;
                 }
         } else
@@ -994,6 +999,8 @@ static void make_request(struct mddev *mddev, struct bio * bio)
         const int rw = bio_data_dir(bio);
         const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
         const unsigned long do_flush_fua = (bio->bi_rw & (REQ_FLUSH | REQ_FUA));
+        const unsigned long do_discard = (bio->bi_rw
+                                          & (REQ_DISCARD | REQ_SECURE));
         struct md_rdev *blocked_rdev;
         struct blk_plug_cb *cb;
         struct raid1_plug_cb *plug = NULL;
@@ -1295,7 +1302,7 @@ read_again:
                                    conf->mirrors[i].rdev->data_offset);
                 mbio->bi_bdev = conf->mirrors[i].rdev->bdev;
                 mbio->bi_end_io = raid1_end_write_request;
-                mbio->bi_rw = WRITE | do_flush_fua | do_sync;
+                mbio->bi_rw = WRITE | do_flush_fua | do_sync | do_discard;
                 mbio->bi_private = r1_bio;
 
                 atomic_inc(&r1_bio->remaining);
@@ -1549,6 +1556,8 @@ static int raid1_add_disk(struct mddev *mddev, struct md_rdev *rdev)
                 clear_bit(Unmerged, &rdev->flags);
         }
         md_integrity_add_rdev(rdev, mddev);
+        if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+                queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
         print_conf(conf);
         return err;
 }
@@ -1867,7 +1876,7 @@ static int process_checks(struct r1bio *r1_bio)
                 } else
                         j = 0;
                 if (j >= 0)
-                        mddev->resync_mismatches += r1_bio->sectors;
+                        atomic64_add(r1_bio->sectors, &mddev->resync_mismatches);
                 if (j < 0 || (test_bit(MD_RECOVERY_CHECK, &mddev->recovery)
                               && test_bit(BIO_UPTODATE, &sbio->bi_flags))) {
                         /* No need to write to this device. */
@@ -2220,6 +2229,7 @@ static void handle_read_error(struct r1conf *conf, struct r1bio *r1_bio)
                 unfreeze_array(conf);
         } else
                 md_error(mddev, conf->mirrors[r1_bio->read_disk].rdev);
+        rdev_dec_pending(conf->mirrors[r1_bio->read_disk].rdev, conf->mddev);
 
         bio = r1_bio->bios[r1_bio->read_disk];
         bdevname(bio->bi_bdev, b);
@@ -2285,8 +2295,9 @@ read_more:
         }
 }
 
-static void raid1d(struct mddev *mddev)
+static void raid1d(struct md_thread *thread)
 {
+        struct mddev *mddev = thread->mddev;
         struct r1bio *r1_bio;
         unsigned long flags;
         struct r1conf *conf = mddev->private;
@@ -2699,7 +2710,7 @@ static struct r1conf *setup_conf(struct mddev *mddev)
                     || disk_idx < 0)
                         continue;
                 if (test_bit(Replacement, &rdev->flags))
-                        disk = conf->mirrors + conf->raid_disks + disk_idx;
+                        disk = conf->mirrors + mddev->raid_disks + disk_idx;
                 else
                         disk = conf->mirrors + disk_idx;
 
@@ -2783,6 +2794,7 @@ static int run(struct mddev *mddev)
         int i;
         struct md_rdev *rdev;
         int ret;
+        bool discard_supported = false;
 
         if (mddev->level != 1) {
                 printk(KERN_ERR "md/raid1:%s: raid level not set to mirroring (%d)\n",
@@ -2812,6 +2824,8 @@ static int run(struct mddev *mddev)
                         continue;
                 disk_stack_limits(mddev->gendisk, rdev->bdev,
                                   rdev->data_offset << 9);
+                if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+                        discard_supported = true;
         }
 
         mddev->degraded = 0;
@@ -2846,6 +2860,13 @@ static int run(struct mddev *mddev)
                 mddev->queue->backing_dev_info.congested_fn = raid1_congested;
                 mddev->queue->backing_dev_info.congested_data = mddev;
                 blk_queue_merge_bvec(mddev->queue, raid1_mergeable_bvec);
+
+                if (discard_supported)
+                        queue_flag_set_unlocked(QUEUE_FLAG_DISCARD,
+                                                mddev->queue);
+                else
+                        queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD,
+                                                  mddev->queue);
         }
 
         ret =  md_integrity_register(mddev);
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 0138a727c1f3..d1295aff4173 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -911,7 +911,12 @@ static void flush_pending_writes(struct r10conf *conf)
                 while (bio) { /* submit pending writes */
                         struct bio *next = bio->bi_next;
                         bio->bi_next = NULL;
-                        generic_make_request(bio);
+                        if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+                            !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
+                                /* Just ignore it */
+                                bio_endio(bio, 0);
+                        else
+                                generic_make_request(bio);
                         bio = next;
                 }
         } else
@@ -1050,6 +1055,44 @@ static sector_t choose_data_offset(struct r10bio *r10_bio,
                 return rdev->new_data_offset;
 }
 
+struct raid10_plug_cb {
+        struct blk_plug_cb      cb;
+        struct bio_list         pending;
+        int                     pending_cnt;
+};
+
+static void raid10_unplug(struct blk_plug_cb *cb, bool from_schedule)
+{
+        struct raid10_plug_cb *plug = container_of(cb, struct raid10_plug_cb,
+                                                   cb);
+        struct mddev *mddev = plug->cb.data;
+        struct r10conf *conf = mddev->private;
+        struct bio *bio;
+
+        if (from_schedule) {
+                spin_lock_irq(&conf->device_lock);
+                bio_list_merge(&conf->pending_bio_list, &plug->pending);
+                conf->pending_count += plug->pending_cnt;
+                spin_unlock_irq(&conf->device_lock);
+                md_wakeup_thread(mddev->thread);
+                kfree(plug);
+                return;
+        }
+
+        /* we aren't scheduling, so we can do the write-out directly. */
+        bio = bio_list_get(&plug->pending);
+        bitmap_unplug(mddev->bitmap);
+        wake_up(&conf->wait_barrier);
+
+        while (bio) { /* submit pending writes */
+                struct bio *next = bio->bi_next;
+                bio->bi_next = NULL;
+                generic_make_request(bio);
+                bio = next;
+        }
+        kfree(plug);
+}
+
 static void make_request(struct mddev *mddev, struct bio * bio)
 {
         struct r10conf *conf = mddev->private;
@@ -1061,8 +1104,12 @@ static void make_request(struct mddev *mddev, struct bio * bio)
         const int rw = bio_data_dir(bio);
         const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
         const unsigned long do_fua = (bio->bi_rw & REQ_FUA);
+        const unsigned long do_discard = (bio->bi_rw
+                                          & (REQ_DISCARD | REQ_SECURE));
         unsigned long flags;
         struct md_rdev *blocked_rdev;
+        struct blk_plug_cb *cb;
+        struct raid10_plug_cb *plug = NULL;
         int sectors_handled;
         int max_sectors;
         int sectors;
@@ -1081,7 +1128,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
                          || conf->prev.near_copies < conf->prev.raid_disks))) {
                 struct bio_pair *bp;
                 /* Sanity check -- queue functions should prevent this happening */
-                if (bio->bi_vcnt != 1 ||
+                if ((bio->bi_vcnt != 1 && bio->bi_vcnt != 0) ||
                     bio->bi_idx != 0)
                         goto bad_map;
                 /* This is a one page bio that upper layers
@@ -1410,15 +1457,26 @@ retry_write:
                                                       conf->mirrors[d].rdev));
                 mbio->bi_bdev = conf->mirrors[d].rdev->bdev;
                 mbio->bi_end_io = raid10_end_write_request;
-                mbio->bi_rw = WRITE | do_sync | do_fua;
+                mbio->bi_rw = WRITE | do_sync | do_fua | do_discard;
                 mbio->bi_private = r10_bio;
 
                 atomic_inc(&r10_bio->remaining);
+
+                cb = blk_check_plugged(raid10_unplug, mddev, sizeof(*plug));
+                if (cb)
+                        plug = container_of(cb, struct raid10_plug_cb, cb);
+                else
+                        plug = NULL;
                 spin_lock_irqsave(&conf->device_lock, flags);
-                bio_list_add(&conf->pending_bio_list, mbio);
-                conf->pending_count++;
+                if (plug) {
+                        bio_list_add(&plug->pending, mbio);
+                        plug->pending_cnt++;
+                } else {
+                        bio_list_add(&conf->pending_bio_list, mbio);
+                        conf->pending_count++;
+                }
                 spin_unlock_irqrestore(&conf->device_lock, flags);
-                if (!mddev_check_plugged(mddev))
+                if (!plug)
                         md_wakeup_thread(mddev->thread);
 
                 if (!r10_bio->devs[i].repl_bio)
@@ -1439,7 +1497,7 @@ retry_write:
                                            conf->mirrors[d].replacement));
                 mbio->bi_bdev = conf->mirrors[d].replacement->bdev;
                 mbio->bi_end_io = raid10_end_write_request;
-                mbio->bi_rw = WRITE | do_sync | do_fua;
+                mbio->bi_rw = WRITE | do_sync | do_fua | do_discard;
                 mbio->bi_private = r10_bio;
 
                 atomic_inc(&r10_bio->remaining);
@@ -1638,7 +1696,7 @@ static int raid10_spare_active(struct mddev *mddev)
                            && !test_bit(Faulty, &tmp->rdev->flags)
                            && !test_and_set_bit(In_sync, &tmp->rdev->flags)) {
                         count++;
-                        sysfs_notify_dirent(tmp->rdev->sysfs_state);
+                        sysfs_notify_dirent_safe(tmp->rdev->sysfs_state);
                 }
         }
         spin_lock_irqsave(&conf->device_lock, flags);
@@ -1725,6 +1783,9 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
                 clear_bit(Unmerged, &rdev->flags);
         }
         md_integrity_add_rdev(rdev, mddev);
+        if (mddev->queue && blk_queue_discard(bdev_get_queue(rdev->bdev)))
+                queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
         print_conf(conf);
         return err;
 }
@@ -1952,7 +2013,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
                                         break;
                         if (j == vcnt)
                                 continue;
-                        mddev->resync_mismatches += r10_bio->sectors;
+                        atomic64_add(r10_bio->sectors, &mddev->resync_mismatches);
                         if (test_bit(MD_RECOVERY_CHECK, &mddev->recovery))
                                 /* Don't fix anything. */
                                 continue;
@@ -2673,8 +2734,9 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
         }
 }
 
-static void raid10d(struct mddev *mddev)
+static void raid10d(struct md_thread *thread)
 {
+        struct mddev *mddev = thread->mddev;
         struct r10bio *r10_bio;
         unsigned long flags;
         struct r10conf *conf = mddev->private;
@@ -3158,7 +3220,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
                                 else {
                                         bad_sectors -= (sector - first_bad);
                                         if (max_sync > bad_sectors)
-                                                max_sync = max_sync;
+                                                max_sync = bad_sectors;
                                         continue;
                                 }
                         }
@@ -3482,6 +3544,7 @@ static int run(struct mddev *mddev)
         sector_t size;
         sector_t min_offset_diff = 0;
         int first = 1;
+        bool discard_supported = false;
 
         if (mddev->private == NULL) {
                 conf = setup_conf(mddev);
@@ -3498,6 +3561,8 @@ static int run(struct mddev *mddev)
 
         chunk_size = mddev->chunk_sectors << 9;
         if (mddev->queue) {
+                blk_queue_max_discard_sectors(mddev->queue,
+                                              mddev->chunk_sectors);
                 blk_queue_io_min(mddev->queue, chunk_size);
                 if (conf->geo.raid_disks % conf->geo.near_copies)
                         blk_queue_io_opt(mddev->queue, chunk_size * conf->geo.raid_disks);
@@ -3543,8 +3608,19 @@ static int run(struct mddev *mddev)
                                           rdev->data_offset << 9);
 
                 disk->head_position = 0;
+
+                if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+                        discard_supported = true;
         }
 
+        if (mddev->queue) {
+                if (discard_supported)
+                        queue_flag_set_unlocked(QUEUE_FLAG_DISCARD,
+                                                mddev->queue);
+                else
+                        queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD,
+                                                  mddev->queue);
+        }
         /* need to check that every block has at least one working mirror */
         if (!enough(conf, -1)) {
                 printk(KERN_ERR "md/raid10:%s: not enough operational mirrors.\n",
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 0689173fd9f5..c5439dce0295 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -551,6 +551,8 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
                                 rw = WRITE_FUA;
                         else
                                 rw = WRITE;
+                        if (test_bit(R5_Discard, &sh->dev[i].flags))
+                                rw |= REQ_DISCARD;
                 } else if (test_and_clear_bit(R5_Wantread, &sh->dev[i].flags))
                         rw = READ;
                 else if (test_and_clear_bit(R5_WantReplace,
@@ -1174,8 +1176,11 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
                                         set_bit(R5_WantFUA, &dev->flags);
                                 if (wbi->bi_rw & REQ_SYNC)
                                         set_bit(R5_SyncIO, &dev->flags);
-                                tx = async_copy_data(1, wbi, dev->page,
-                                        dev->sector, tx);
+                                if (wbi->bi_rw & REQ_DISCARD)
+                                        set_bit(R5_Discard, &dev->flags);
+                                else
+                                        tx = async_copy_data(1, wbi, dev->page,
+                                                dev->sector, tx);
                                 wbi = r5_next_bio(wbi, dev->sector);
                         }
                 }
@@ -1191,7 +1196,7 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
         int pd_idx = sh->pd_idx;
         int qd_idx = sh->qd_idx;
         int i;
-        bool fua = false, sync = false;
+        bool fua = false, sync = false, discard = false;
 
         pr_debug("%s: stripe %llu\n", __func__,
                 (unsigned long long)sh->sector);
@@ -1199,13 +1204,15 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
         for (i = disks; i--; ) {
                 fua |= test_bit(R5_WantFUA, &sh->dev[i].flags);
                 sync |= test_bit(R5_SyncIO, &sh->dev[i].flags);
+                discard |= test_bit(R5_Discard, &sh->dev[i].flags);
         }
 
         for (i = disks; i--; ) {
                 struct r5dev *dev = &sh->dev[i];
 
                 if (dev->written || i == pd_idx || i == qd_idx) {
-                        set_bit(R5_UPTODATE, &dev->flags);
+                        if (!discard)
+                                set_bit(R5_UPTODATE, &dev->flags);
                         if (fua)
                                 set_bit(R5_WantFUA, &dev->flags);
                         if (sync)
@@ -1241,6 +1248,18 @@ ops_run_reconstruct5(struct stripe_head *sh, struct raid5_percpu *percpu,
         pr_debug("%s: stripe %llu\n", __func__,
                 (unsigned long long)sh->sector);
 
+        for (i = 0; i < sh->disks; i++) {
+                if (pd_idx == i)
+                        continue;
+                if (!test_bit(R5_Discard, &sh->dev[i].flags))
+                        break;
+        }
+        if (i >= sh->disks) {
+                atomic_inc(&sh->count);
+                set_bit(R5_Discard, &sh->dev[pd_idx].flags);
+                ops_complete_reconstruct(sh);
+                return;
+        }
         /* check if prexor is active which means only process blocks
          * that are part of a read-modify-write (written)
          */
@@ -1285,10 +1304,24 @@ ops_run_reconstruct6(struct stripe_head *sh, struct raid5_percpu *percpu,
 {
         struct async_submit_ctl submit;
         struct page **blocks = percpu->scribble;
-        int count;
+        int count, i;
 
         pr_debug("%s: stripe %llu\n", __func__, (unsigned long long)sh->sector);
 
+        for (i = 0; i < sh->disks; i++) {
+                if (sh->pd_idx == i || sh->qd_idx == i)
+                        continue;
+                if (!test_bit(R5_Discard, &sh->dev[i].flags))
+                        break;
+        }
+        if (i >= sh->disks) {
+                atomic_inc(&sh->count);
+                set_bit(R5_Discard, &sh->dev[sh->pd_idx].flags);
+                set_bit(R5_Discard, &sh->dev[sh->qd_idx].flags);
+                ops_complete_reconstruct(sh);
+                return;
+        }
+
         count = set_syndrome_sources(blocks, sh);
 
         atomic_inc(&sh->count);
@@ -2408,11 +2441,11 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
                 if (sector >= sh->dev[dd_idx].sector + STRIPE_SECTORS)
                         set_bit(R5_OVERWRITE, &sh->dev[dd_idx].flags);
         }
-        spin_unlock_irq(&sh->stripe_lock);
 
         pr_debug("added bi b#%llu to stripe s#%llu, disk %d.\n",
                 (unsigned long long)(*bip)->bi_sector,
                 (unsigned long long)sh->sector, dd_idx);
+        spin_unlock_irq(&sh->stripe_lock);
 
         if (conf->mddev->bitmap && firstwrite) {
                 bitmap_startwrite(conf->mddev->bitmap, sh->sector,
@@ -2479,10 +2512,8 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
                 bi = sh->dev[i].towrite;
                 sh->dev[i].towrite = NULL;
                 spin_unlock_irq(&sh->stripe_lock);
-                if (bi) {
-                        s->to_write--;
+                if (bi)
                         bitmap_end = 1;
-                }
 
                 if (test_and_clear_bit(R5_Overlap, &sh->dev[i].flags))
                         wake_up(&conf->wait_for_overlap);
@@ -2524,11 +2555,12 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
                 if (!test_bit(R5_Wantfill, &sh->dev[i].flags) &&
                     (!test_bit(R5_Insync, &sh->dev[i].flags) ||
                       test_bit(R5_ReadError, &sh->dev[i].flags))) {
+                        spin_lock_irq(&sh->stripe_lock);
                         bi = sh->dev[i].toread;
                         sh->dev[i].toread = NULL;
+                        spin_unlock_irq(&sh->stripe_lock);
                         if (test_and_clear_bit(R5_Overlap, &sh->dev[i].flags))
                                 wake_up(&conf->wait_for_overlap);
-                        if (bi) s->to_read--;
                         while (bi && bi->bi_sector <
                                sh->dev[i].sector + STRIPE_SECTORS) {
                                 struct bio *nextbi =
@@ -2741,7 +2773,8 @@ static void handle_stripe_clean_event(struct r5conf *conf,
                 if (sh->dev[i].written) {
                         dev = &sh->dev[i];
                         if (!test_bit(R5_LOCKED, &dev->flags) &&
-                                test_bit(R5_UPTODATE, &dev->flags)) {
+                            (test_bit(R5_UPTODATE, &dev->flags) ||
+                             test_and_clear_bit(R5_Discard, &dev->flags))) {
                                 /* We can return any write requests */
                                 struct bio *wbi, *wbi2;
                                 pr_debug("Return write for disc %d\n", i);
@@ -2775,12 +2808,25 @@ static void handle_stripe_dirtying(struct r5conf *conf,
                                    int disks)
 {
         int rmw = 0, rcw = 0, i;
-        if (conf->max_degraded == 2) {
-                /* RAID6 requires 'rcw' in current implementation
-                 * Calculate the real rcw later - for now fake it
+        sector_t recovery_cp = conf->mddev->recovery_cp;
+
+        /* RAID6 requires 'rcw' in current implementation.
+         * Otherwise, check whether resync is now happening or should start.
+         * If yes, then the array is dirty (after unclean shutdown or
+         * initial creation), so parity in some stripes might be inconsistent.
+         * In this case, we need to always do reconstruct-write, to ensure
+         * that in case of drive failure or read-error correction, we
+         * generate correct data from the parity.
+         */
+        if (conf->max_degraded == 2 ||
+            (recovery_cp < MaxSector && sh->sector >= recovery_cp)) {
+                /* Calculate the real rcw later - for now make it
                  * look like rcw is cheaper
                  */
                 rcw = 1; rmw = 2;
+                pr_debug("force RCW max_degraded=%u, recovery_cp=%llu sh->sector=%llu\n",
+                         conf->max_degraded, (unsigned long long)recovery_cp,
+                         (unsigned long long)sh->sector);
         } else for (i = disks; i--; ) {
                 /* would I have to read this buffer for read_modify_write */
                 struct r5dev *dev = &sh->dev[i];
@@ -2932,7 +2978,7 @@ static void handle_parity_checks5(struct r5conf *conf, struct stripe_head *sh,
                          */
                         set_bit(STRIPE_INSYNC, &sh->state);
                 else {
-                        conf->mddev->resync_mismatches += STRIPE_SECTORS;
+                        atomic64_add(STRIPE_SECTORS, &conf->mddev->resync_mismatches);
                         if (test_bit(MD_RECOVERY_CHECK, &conf->mddev->recovery))
                                 /* don't try to repair!! */
                                 set_bit(STRIPE_INSYNC, &sh->state);
@@ -3084,7 +3130,7 @@ static void handle_parity_checks6(struct r5conf *conf, struct stripe_head *sh,
                                  */
                         }
                 } else {
-                        conf->mddev->resync_mismatches += STRIPE_SECTORS;
+                        atomic64_add(STRIPE_SECTORS, &conf->mddev->resync_mismatches);
                         if (test_bit(MD_RECOVERY_CHECK, &conf->mddev->recovery))
                                 /* don't try to repair!! */
                                 set_bit(STRIPE_INSYNC, &sh->state);
@@ -3459,10 +3505,12 @@ static void handle_stripe(struct stripe_head *sh)
         if (s.written &&
             (s.p_failed || ((test_bit(R5_Insync, &pdev->flags)
                              && !test_bit(R5_LOCKED, &pdev->flags)
-                             && test_bit(R5_UPTODATE, &pdev->flags)))) &&
+                             && (test_bit(R5_UPTODATE, &pdev->flags) ||
+                                 test_bit(R5_Discard, &pdev->flags))))) &&
             (s.q_failed || ((test_bit(R5_Insync, &qdev->flags)
                              && !test_bit(R5_LOCKED, &qdev->flags)
-                             && test_bit(R5_UPTODATE, &qdev->flags)))))
+                             && (test_bit(R5_UPTODATE, &qdev->flags) ||
+                                 test_bit(R5_Discard, &qdev->flags))))))
                 handle_stripe_clean_event(conf, sh, disks, &s.return_bi);
 
         /* Now we might consider reading some blocks, either to check/generate
@@ -3489,9 +3537,11 @@ static void handle_stripe(struct stripe_head *sh)
                 /* All the 'written' buffers and the parity block are ready to
                  * be written back to disk
                  */
-                BUG_ON(!test_bit(R5_UPTODATE, &sh->dev[sh->pd_idx].flags));
+                BUG_ON(!test_bit(R5_UPTODATE, &sh->dev[sh->pd_idx].flags) &&
+                       !test_bit(R5_Discard, &sh->dev[sh->pd_idx].flags));
                 BUG_ON(sh->qd_idx >= 0 &&
-                       !test_bit(R5_UPTODATE, &sh->dev[sh->qd_idx].flags));
+                       !test_bit(R5_UPTODATE, &sh->dev[sh->qd_idx].flags) &&
+                       !test_bit(R5_Discard, &sh->dev[sh->qd_idx].flags));
                 for (i = disks; i--; ) {
                         struct r5dev *dev = &sh->dev[i];
                         if (test_bit(R5_LOCKED, &dev->flags) &&
@@ -4072,6 +4122,88 @@ static void release_stripe_plug(struct mddev *mddev,
                 release_stripe(sh);
 }
 
+static void make_discard_request(struct mddev *mddev, struct bio *bi)
+{
+        struct r5conf *conf = mddev->private;
+        sector_t logical_sector, last_sector;
+        struct stripe_head *sh;
+        int remaining;
+        int stripe_sectors;
+
+        if (mddev->reshape_position != MaxSector)
+                /* Skip discard while reshape is happening */
+                return;
+
+        logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
+        last_sector = bi->bi_sector + (bi->bi_size>>9);
+
+        bi->bi_next = NULL;
+        bi->bi_phys_segments = 1; /* over-loaded to count active stripes */
+
+        stripe_sectors = conf->chunk_sectors *
+                (conf->raid_disks - conf->max_degraded);
+        logical_sector = DIV_ROUND_UP_SECTOR_T(logical_sector,
+                                               stripe_sectors);
+        sector_div(last_sector, stripe_sectors);
+
+        logical_sector *= conf->chunk_sectors;
+        last_sector *= conf->chunk_sectors;
+
+        for (; logical_sector < last_sector;
+             logical_sector += STRIPE_SECTORS) {
+                DEFINE_WAIT(w);
+                int d;
+        again:
+                sh = get_active_stripe(conf, logical_sector, 0, 0, 0);
+                prepare_to_wait(&conf->wait_for_overlap, &w,
+                                TASK_UNINTERRUPTIBLE);
+                spin_lock_irq(&sh->stripe_lock);
+                for (d = 0; d < conf->raid_disks; d++) {
+                        if (d == sh->pd_idx || d == sh->qd_idx)
+                                continue;
+                        if (sh->dev[d].towrite || sh->dev[d].toread) {
+                                set_bit(R5_Overlap, &sh->dev[d].flags);
+                                spin_unlock_irq(&sh->stripe_lock);
+                                release_stripe(sh);
+                                schedule();
+                                goto again;
+                        }
+                }
+                finish_wait(&conf->wait_for_overlap, &w);
+                for (d = 0; d < conf->raid_disks; d++) {
+                        if (d == sh->pd_idx || d == sh->qd_idx)
+                                continue;
+                        sh->dev[d].towrite = bi;
+                        set_bit(R5_OVERWRITE, &sh->dev[d].flags);
+                        raid5_inc_bi_active_stripes(bi);
+                }
+                spin_unlock_irq(&sh->stripe_lock);
+                if (conf->mddev->bitmap) {
+                        for (d = 0;
+                             d < conf->raid_disks - conf->max_degraded;
+                             d++)
+                                bitmap_startwrite(mddev->bitmap,
+                                                  sh->sector,
+                                                  STRIPE_SECTORS,
+                                                  0);
+                        sh->bm_seq = conf->seq_flush + 1;
+                        set_bit(STRIPE_BIT_DELAY, &sh->state);
+                }
+
+                set_bit(STRIPE_HANDLE, &sh->state);
+                clear_bit(STRIPE_DELAYED, &sh->state);
+                if (!test_and_set_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
+                        atomic_inc(&conf->preread_active_stripes);
+                release_stripe_plug(mddev, sh);
+        }
+
+        remaining = raid5_dec_bi_active_stripes(bi);
+        if (remaining == 0) {
+                md_write_end(mddev);
+                bio_endio(bi, 0);
+        }
+}
+
 static void make_request(struct mddev *mddev, struct bio * bi)
 {
         struct r5conf *conf = mddev->private;
@@ -4094,6 +4226,11 @@ static void make_request(struct mddev *mddev, struct bio * bi)
              chunk_aligned_read(mddev,bi))
                 return;
 
+        if (unlikely(bi->bi_rw & REQ_DISCARD)) {
+                make_discard_request(mddev, bi);
+                return;
+        }
+
         logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
         last_sector = bi->bi_sector + (bi->bi_size>>9);
         bi->bi_next = NULL;
@@ -4630,8 +4767,9 @@ static int handle_active_stripes(struct r5conf *conf)
  * During the scan, completed stripes are saved for us by the interrupt
  * handler, so that they will not have to wait for our next wakeup.
  */
-static void raid5d(struct mddev *mddev)
+static void raid5d(struct md_thread *thread)
 {
+        struct mddev *mddev = thread->mddev;
         struct r5conf *conf = mddev->private;
         int handled;
         struct blk_plug plug;
@@ -5366,6 +5504,7 @@ static int run(struct mddev *mddev)
 
         if (mddev->queue) {
                 int chunk_size;
+                bool discard_supported = true;
                 /* read-ahead size must cover two whole stripes, which
                  * is 2 * (datadisks) * chunksize where 'n' is the
                  * number of raid devices
@@ -5385,13 +5524,48 @@ static int run(struct mddev *mddev)
                 blk_queue_io_min(mddev->queue, chunk_size);
                 blk_queue_io_opt(mddev->queue, chunk_size *
                                  (conf->raid_disks - conf->max_degraded));
+                /*
+                 * We can only discard a whole stripe. It doesn't make sense to
+                 * discard data disk but write parity disk
+                 */
+                stripe = stripe * PAGE_SIZE;
+                mddev->queue->limits.discard_alignment = stripe;
+                mddev->queue->limits.discard_granularity = stripe;
+                /*
+                 * unaligned part of discard request will be ignored, so can't
+                 * guarantee discard_zerors_data
+                 */
+                mddev->queue->limits.discard_zeroes_data = 0;
 
                 rdev_for_each(rdev, mddev) {
                         disk_stack_limits(mddev->gendisk, rdev->bdev,
                                           rdev->data_offset << 9);
                         disk_stack_limits(mddev->gendisk, rdev->bdev,
                                           rdev->new_data_offset << 9);
+                        /*
+                         * discard_zeroes_data is required, otherwise data
+                         * could be lost. Consider a scenario: discard a stripe
+                         * (the stripe could be inconsistent if
+                         * discard_zeroes_data is 0); write one disk of the
+                         * stripe (the stripe could be inconsistent again
+                         * depending on which disks are used to calculate
+                         * parity); the disk is broken; The stripe data of this
+                         * disk is lost.
+                         */
+                        if (!blk_queue_discard(bdev_get_queue(rdev->bdev)) ||
+                            !bdev_get_queue(rdev->bdev)->
+                                                limits.discard_zeroes_data)
+                                discard_supported = false;
                 }
+
+                if (discard_supported &&
+                   mddev->queue->limits.max_discard_sectors >= stripe &&
+                   mddev->queue->limits.discard_granularity >= stripe)
+                        queue_flag_set_unlocked(QUEUE_FLAG_DISCARD,
+                                                mddev->queue);
+                else
+                        queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD,
+                                                mddev->queue);
         }
 
         return 0;
@@ -5702,7 +5876,8 @@ static int check_reshape(struct mddev *mddev)
         if (!check_stripe_cache(mddev))
                 return -ENOSPC;
 
-        return resize_stripes(conf, conf->raid_disks + mddev->delta_disks);
+        return resize_stripes(conf, (conf->previous_raid_disks
+                                     + mddev->delta_disks));
 }
 
 static int raid5_start_reshape(struct mddev *mddev)
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index a9fc24901eda..18b2c4a8a1fd 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -298,6 +298,7 @@ enum r5dev_flags {
         R5_WantReplace, /* We need to update the replacement, we have read
                          * data in, and now is a good time to write it out.
                          */
+        R5_Discard,     /* Discard the stripe */
 };
 
 /*
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index dd13e3a4c272..4ef0d80b57f4 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -163,19 +163,21 @@ source "drivers/media/common/Kconfig"
 #
 
 config MEDIA_SUBDRV_AUTOSELECT
-        bool "Autoselect analog and hybrid tuner modules to build"
-        depends on MEDIA_TUNER
+        bool "Autoselect tuners and i2c modules to build"
+        depends on MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT || MEDIA_CAMERA_SUPPORT
         default y
         help
-          By default, a TV driver auto-selects all possible tuners
-          thar could be used by the driver.
+          By default, a media driver auto-selects all possible i2c
+          devices that are used by any of the supported devices.
 
           This is generally the right thing to do, except when there
-          are strict constraints with regards to the kernel size.
+          are strict constraints with regards to the kernel size,
+          like on embedded systems.
 
-          Use this option with care, as deselecting tuner drivers which
-          are in fact necessary will result in TV devices which cannot
-          be tuned due to lack of the tuning driver.
+          Use this option with care, as deselecting ancillary drivers which
+          are, in fact, necessary will result in the lack of the needed
+          functionality for your device (it may not tune or may not have
+          the need demodulers).
 
           If unsure say Y.
 
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 8f58f241c10d..7e92793260f0 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -966,6 +966,8 @@ static int dvb_frontend_clear_cache(struct dvb_frontend *fe)
                 break;
         }
 
+        c->lna = LNA_AUTO;
+
         return 0;
 }
 
@@ -1054,6 +1056,8 @@ static struct dtv_cmds_h dtv_cmds[DTV_MAX_COMMAND + 1] = {
         _DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_B, 0, 0),
         _DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_C, 0, 0),
         _DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_D, 0, 0),
+
+        _DTV_CMD(DTV_LNA, 0, 0),
 };
 
 static void dtv_property_dump(struct dvb_frontend *fe, struct dtv_property *tvp)
@@ -1440,6 +1444,10 @@ static int dtv_property_process_get(struct dvb_frontend *fe,
                 tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_d;
                 break;
 
+        case DTV_LNA:
+                tvp->u.data = c->lna;
+                break;
+
         default:
                 return -EINVAL;
         }
@@ -1731,10 +1739,6 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
         case DTV_INTERLEAVING:
                 c->interleaving = tvp->u.data;
                 break;
-        case DTV_LNA:
-                if (fe->ops.set_lna)
-                        r = fe->ops.set_lna(fe, tvp->u.data);
-                break;
 
         /* ISDB-T Support here */
         case DTV_ISDBT_PARTIAL_RECEPTION:
@@ -1806,6 +1810,12 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
                 fe->dtv_property_cache.atscmh_rs_frame_ensemble = tvp->u.data;
                 break;
 
+        case DTV_LNA:
+                c->lna = tvp->u.data;
+                if (fe->ops.set_lna)
+                        r = fe->ops.set_lna(fe);
+                break;
+
         default:
                 return -EINVAL;
         }
@@ -2309,7 +2319,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
                 fepriv->tune_mode_flags = (unsigned long) parg;
                 err = 0;
                 break;
-        };
+        }
 
         return err;
 }
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index 44a445cee74f..97112cd88a17 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -303,7 +303,7 @@ struct dvb_frontend_ops {
         int (*dishnetwork_send_legacy_command)(struct dvb_frontend* fe, unsigned long cmd);
         int (*i2c_gate_ctrl)(struct dvb_frontend* fe, int enable);
         int (*ts_bus_ctrl)(struct dvb_frontend* fe, int acquire);
-        int (*set_lna)(struct dvb_frontend *, int);
+        int (*set_lna)(struct dvb_frontend *);
 
         /* These callbacks are for devices that implement their own
          * tuning algorithms, rather than a simple swzigzag
@@ -391,6 +391,8 @@ struct dtv_frontend_properties {
         u8                      atscmh_sccc_code_mode_b;
         u8                      atscmh_sccc_code_mode_c;
         u8                      atscmh_sccc_code_mode_d;
+
+        u32                     lna;
 };
 
 struct dvb_frontend {
diff --git a/drivers/media/dvb-frontends/a8293.c b/drivers/media/dvb-frontends/a8293.c
index cff44a389b40..74fbb5d58bed 100644
--- a/drivers/media/dvb-frontends/a8293.c
+++ b/drivers/media/dvb-frontends/a8293.c
@@ -90,7 +90,7 @@ static int a8293_set_voltage(struct dvb_frontend *fe,
         default:
                 ret = -EINVAL;
                 goto err;
-        };
+        }
 
         ret = a8293_wr(priv, &priv->reg[0], 1);
         if (ret)
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index e9f04a36577b..a204f2828820 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -241,7 +241,7 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
                                 KBUILD_MODNAME, gpio);
                 ret = -EINVAL;
                 goto err;
-        };
+        }
 
         switch (gpio) {
         case 0:
@@ -253,7 +253,7 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
         default:
                 pos = 4;
                 break;
-        };
+        }
 
         ret = af9013_wr_reg_bits(state, addr, pos, 4, gpioval);
         if (ret)
@@ -726,7 +726,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
         default:
                 dev_dbg(&state->i2c->dev, "%s: invalid hierarchy\n", __func__);
                 auto_mode = 1;
-        };
+        }
 
         switch (c->modulation) {
         case QAM_AUTO:
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index 8162d939c4b2..464ad878490b 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -408,7 +408,7 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 {
         struct af9033_state *state = fe->demodulator_priv;
         struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-        int ret, i, spec_inv;
+        int ret, i, spec_inv, sampling_freq;
         u8 tmp, buf[3], bandwidth_reg_val;
         u32 if_frequency, freq_cw, adc_freq;
 
@@ -465,18 +465,20 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
                 else
                         if_frequency = 0;
 
-                while (if_frequency > (adc_freq / 2))
-                        if_frequency -= adc_freq;
+                sampling_freq = if_frequency;
 
-                if (if_frequency >= 0)
+                while (sampling_freq > (adc_freq / 2))
+                        sampling_freq -= adc_freq;
+
+                if (sampling_freq >= 0)
                         spec_inv *= -1;
                 else
-                        if_frequency *= -1;
+                        sampling_freq *= -1;
 
-                freq_cw = af9033_div(state, if_frequency, adc_freq, 23ul);
+                freq_cw = af9033_div(state, sampling_freq, adc_freq, 23ul);
 
                 if (spec_inv == -1)
-                        freq_cw *= -1;
+                        freq_cw = 0x800000 - freq_cw;
 
                 /* get adc multiplies */
                 ret = af9033_rd_reg(state, 0x800045, &tmp);
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 033cd7ad3ca2..1b77909c0c71 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -527,7 +527,7 @@ static int bcm3510_set_frontend(struct dvb_frontend *fe)
                         cmd.ACQUIRE1.IF_FREQ = 0x0;
                 default:
                         return -EINVAL;
-        };
+        }
         cmd.ACQUIRE0.OFFSET = 0;
         cmd.ACQUIRE0.NTSCSWEEP = 1;
         cmd.ACQUIRE0.FA = 1;
diff --git a/drivers/media/dvb-frontends/cx24110.c b/drivers/media/dvb-frontends/cx24110.c
index 3180f5b2a6a6..0cd6927e654c 100644
--- a/drivers/media/dvb-frontends/cx24110.c
+++ b/drivers/media/dvb-frontends/cx24110.c
@@ -218,7 +218,7 @@ static int cx24110_set_fec (struct cx24110_state* state, fe_code_rate_t fec)
            } else
                    return -EOPNOTSUPP;
 /* fixme (low): which is the correct return code? */
-        };
+        }
         return 0;
 }
 
@@ -275,7 +275,7 @@ static int cx24110_set_symbolrate (struct cx24110_state* state, u32 srate)
                 cx24110_writereg(state,0x07,tmp|0x3);
                 cx24110_writereg(state,0x06,0x78);
                 fclk=90999000UL;
-        };
+        }
         dprintk("cx24110 debug: fclk %d Hz\n",fclk);
         /* we need to divide two integers with approx. 27 bits in 32 bit
            arithmetic giving a 25 bit result */
@@ -362,7 +362,7 @@ static int cx24110_initfe(struct dvb_frontend* fe)
 
         for(i = 0; i < ARRAY_SIZE(cx24110_regdata); i++) {
                 cx24110_writereg(state, cx24110_regdata[i].reg, cx24110_regdata[i].data);
-        };
+        }
 
         return 0;
 }
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 42648643693e..9b658c1cf39a 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -688,7 +688,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
 {
         struct cxd2820r_priv *priv;
         int ret;
-        u8 tmp, gpio[GPIO_COUNT];
+        u8 tmp;
 
         priv = kzalloc(sizeof(struct cxd2820r_priv), GFP_KERNEL);
         if (!priv) {
@@ -735,6 +735,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
                  * Use static GPIO configuration if GPIOLIB is undefined.
                  * This is fallback condition.
                  */
+                u8 gpio[GPIO_COUNT];
                 gpio[0] = (*gpio_chip_base >> 0) & 0x07;
                 gpio[1] = (*gpio_chip_base >> 3) & 0x07;
                 gpio[2] = 0;
diff --git a/drivers/media/dvb-frontends/drxd_hard.c b/drivers/media/dvb-frontends/drxd_hard.c
index f380eb43e9d5..6d9853750d2b 100644
--- a/drivers/media/dvb-frontends/drxd_hard.c
+++ b/drivers/media/dvb-frontends/drxd_hard.c
@@ -991,7 +991,7 @@ static int HI_Command(struct drxd_state *state, u16 cmd, u16 * pResult)
                 if (nrRetries > DRXD_MAX_RETRIES) {
                         status = -1;
                         break;
-                };
+                }
                 status = Read16(state, HI_RA_RAM_SRV_CMD__A, &waitCmd, 0);
         } while (waitCmd != 0);
 
diff --git a/drivers/media/dvb-frontends/ds3000.c b/drivers/media/dvb-frontends/ds3000.c
index 4c8ac2657c4a..5b639087ce45 100644
--- a/drivers/media/dvb-frontends/ds3000.c
+++ b/drivers/media/dvb-frontends/ds3000.c
@@ -30,6 +30,7 @@
 #include "ds3000.h"
 
 static int debug;
+static int force_fw_upload;
 
 #define dprintk(args...) \
         do { \
@@ -392,11 +393,13 @@ static int ds3000_firmware_ondemand(struct dvb_frontend *fe)
 
         dprintk("%s()\n", __func__);
 
-        if (ds3000_readreg(state, 0xb2) <= 0)
+        ret = ds3000_readreg(state, 0xb2);
+        if (ret < 0)
                 return ret;
 
-        if (state->skip_fw_load)
-                return 0;
+        if (state->skip_fw_load || !force_fw_upload)
+                return 0;       /* Firmware already uploaded, skipping */
+
         /* Load firmware */
         /* request the firmware, this will block until someone uploads it */
         printk(KERN_INFO "%s: Waiting for firmware upload (%s)...\n", __func__,
@@ -1306,6 +1309,9 @@ static struct dvb_frontend_ops ds3000_ops = {
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Activates frontend debugging (default:0)");
 
+module_param(force_fw_upload, int, 0644);
+MODULE_PARM_DESC(force_fw_upload, "Force firmware upload (default:0)");
+
 MODULE_DESCRIPTION("DVB Frontend module for Montage Technology "
                         "DS3000/TS2020 hardware");
 MODULE_AUTHOR("Konstantin Dimitrov");
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.c b/drivers/media/dvb-frontends/dvb_dummy_fe.c
index dcfc902c8678..d5acc304786b 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.c
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.c
@@ -121,16 +121,13 @@ struct dvb_frontend* dvb_dummy_fe_ofdm_attach(void)
 
         /* allocate memory for the internal state */
         state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-        if (state == NULL) goto error;
+        if (!state)
+                return NULL;
 
         /* create dvb_frontend */
         memcpy(&state->frontend.ops, &dvb_dummy_fe_ofdm_ops, sizeof(struct dvb_frontend_ops));
         state->frontend.demodulator_priv = state;
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_qpsk_ops;
@@ -141,16 +138,13 @@ struct dvb_frontend *dvb_dummy_fe_qpsk_attach(void)
 
         /* allocate memory for the internal state */
         state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-        if (state == NULL) goto error;
+        if (!state)
+                return NULL;
 
         /* create dvb_frontend */
         memcpy(&state->frontend.ops, &dvb_dummy_fe_qpsk_ops, sizeof(struct dvb_frontend_ops));
         state->frontend.demodulator_priv = state;
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_qam_ops;
@@ -161,16 +155,13 @@ struct dvb_frontend *dvb_dummy_fe_qam_attach(void)
 
         /* allocate memory for the internal state */
         state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-        if (state == NULL) goto error;
+        if (!state)
+                return NULL;
 
         /* create dvb_frontend */
         memcpy(&state->frontend.ops, &dvb_dummy_fe_qam_ops, sizeof(struct dvb_frontend_ops));
         state->frontend.demodulator_priv = state;
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_ofdm_ops = {
diff --git a/drivers/media/dvb-frontends/isl6405.c b/drivers/media/dvb-frontends/isl6405.c
index 33d33f4d8867..0c642a5bf823 100644
--- a/drivers/media/dvb-frontends/isl6405.c
+++ b/drivers/media/dvb-frontends/isl6405.c
@@ -77,7 +77,7 @@ static int isl6405_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
                         break;
                 default:
                         return -EINVAL;
-                };
+                }
         }
         isl6405->config |= isl6405->override_or;
         isl6405->config &= isl6405->override_and;
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index 684c8ec166cb..0cb3f0f74c9c 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -63,7 +63,7 @@ static int isl6421_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         isl6421->config |= isl6421->override_or;
         isl6421->config &= isl6421->override_and;
diff --git a/drivers/media/dvb-frontends/itd1000.c b/drivers/media/dvb-frontends/itd1000.c
index 316457584fe7..c1c3400b2173 100644
--- a/drivers/media/dvb-frontends/itd1000.c
+++ b/drivers/media/dvb-frontends/itd1000.c
@@ -231,7 +231,7 @@ static void itd1000_set_lo(struct itd1000_state *state, u32 freq_khz)
         state->frequency = ((plln * 1000) + (pllf * 1000)/1048576) * 2*FREF;
         itd_dbg("frequency: %dkHz (wanted) %dkHz (set), PLLF = %d, PLLN = %d\n", freq_khz, state->frequency, pllf, plln);
 
-        itd1000_write_reg(state, PLLNH, 0x80); /* PLLNH */;
+        itd1000_write_reg(state, PLLNH, 0x80); /* PLLNH */
         itd1000_write_reg(state, PLLNL, plln & 0xff);
         itd1000_write_reg(state, PLLFH, (itd1000_read_reg(state, PLLFH) & 0xf0) | ((pllf >> 16) & 0x0f));
         itd1000_write_reg(state, PLLFM, (pllf >> 8) & 0xff);
diff --git a/drivers/media/dvb-frontends/lg2160.c b/drivers/media/dvb-frontends/lg2160.c
index cc11260e99df..5fd14f840ab0 100644
--- a/drivers/media/dvb-frontends/lg2160.c
+++ b/drivers/media/dvb-frontends/lg2160.c
@@ -1421,8 +1421,8 @@ struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
                config ? config->i2c_addr : 0);
 
         state = kzalloc(sizeof(struct lg216x_state), GFP_KERNEL);
-        if (state == NULL)
-                goto fail;
+        if (!state)
+                return NULL;
 
         state->cfg = config;
         state->i2c_adap = i2c_adap;
@@ -1449,10 +1449,6 @@ struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
         state->frontend.dtv_property_cache.atscmh_parade_id = 1;
 
         return &state->frontend;
-fail:
-        lg_warn("unable to detect LG216x hardware\n");
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL(lg2160_attach);
 
diff --git a/drivers/media/dvb-frontends/lnbp21.c b/drivers/media/dvb-frontends/lnbp21.c
index 13437259eeac..f3ba7b5faa2e 100644
--- a/drivers/media/dvb-frontends/lnbp21.c
+++ b/drivers/media/dvb-frontends/lnbp21.c
@@ -65,7 +65,7 @@ static int lnbp21_set_voltage(struct dvb_frontend *fe,
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         lnbp21->config |= lnbp21->override_or;
         lnbp21->config &= lnbp21->override_and;
@@ -108,7 +108,7 @@ static int lnbp21_set_tone(struct dvb_frontend *fe,
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         lnbp21->config |= lnbp21->override_or;
         lnbp21->config &= lnbp21->override_and;
diff --git a/drivers/media/dvb-frontends/lnbp22.c b/drivers/media/dvb-frontends/lnbp22.c
index 84ad0390a4a1..c463da7f6dcc 100644
--- a/drivers/media/dvb-frontends/lnbp22.c
+++ b/drivers/media/dvb-frontends/lnbp22.c
@@ -73,7 +73,7 @@ static int lnbp22_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         dprintk(1, "%s: 0x%02x)\n", __func__, lnbp22->config[3]);
         return (i2c_transfer(lnbp22->i2c, &msg, 1) == 1) ? 0 : -EIO;
diff --git a/drivers/media/dvb-frontends/s5h1432.c b/drivers/media/dvb-frontends/s5h1432.c
index 8352ce1c9556..6ec16a243741 100644
--- a/drivers/media/dvb-frontends/s5h1432.c
+++ b/drivers/media/dvb-frontends/s5h1432.c
@@ -351,8 +351,8 @@ struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
         printk(KERN_INFO " Enter s5h1432_attach(). attach success!\n");
         /* allocate memory for the internal state */
         state = kmalloc(sizeof(struct s5h1432_state), GFP_KERNEL);
-        if (state == NULL)
-                goto error;
+        if (!state)
+                return NULL;
 
         /* setup the state */
         state->config = config;
@@ -367,10 +367,6 @@ struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
         state->frontend.demodulator_priv = state;
 
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL(s5h1432_attach);
 
diff --git a/drivers/media/dvb-frontends/s921.c b/drivers/media/dvb-frontends/s921.c
index cd2288c07147..a271ac3eaec0 100644
--- a/drivers/media/dvb-frontends/s921.c
+++ b/drivers/media/dvb-frontends/s921.c
@@ -487,9 +487,9 @@ struct dvb_frontend *s921_attach(const struct s921_config *config,
                 kzalloc(sizeof(struct s921_state), GFP_KERNEL);
 
         dprintk("\n");
-        if (state == NULL) {
+        if (!state) {
                 rc("Unable to kzalloc\n");
-                goto rcor;
+                return NULL;
         }
 
         /* setup the state */
@@ -502,11 +502,6 @@ struct dvb_frontend *s921_attach(const struct s921_config *config,
         state->frontend.demodulator_priv = state;
 
         return &state->frontend;
-
-rcor:
-        kfree(state);
-
-        return NULL;
 }
 EXPORT_SYMBOL(s921_attach);
 
diff --git a/drivers/media/dvb-frontends/si21xx.c b/drivers/media/dvb-frontends/si21xx.c
index a68a64800df7..73b47cc6a13b 100644
--- a/drivers/media/dvb-frontends/si21xx.c
+++ b/drivers/media/dvb-frontends/si21xx.c
@@ -343,7 +343,7 @@ static int si21xx_wait_diseqc_idle(struct si21xx_state *state, int timeout)
                         return -ETIMEDOUT;
                 }
                 msleep(10);
-        };
+        }
 
         return 0;
 }
@@ -472,7 +472,7 @@ static int si21xx_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 }
 
 static int si21xx_init(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/sp8870.c b/drivers/media/dvb-frontends/sp8870.c
index e37274c8f14e..2aa8ef76eba2 100644
--- a/drivers/media/dvb-frontends/sp8870.c
+++ b/drivers/media/dvb-frontends/sp8870.c
@@ -188,7 +188,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->hierarchy) {
         case HIERARCHY_NONE:
@@ -207,7 +207,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->code_rate_HP) {
         case FEC_1_2:
@@ -229,7 +229,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         if (known_parameters)
                 *reg0xc05 |= (2 << 1);  /* use specified parameters */
diff --git a/drivers/media/dvb-frontends/sp887x.c b/drivers/media/dvb-frontends/sp887x.c
index f4096ccb226e..1bb81b5ae6e0 100644
--- a/drivers/media/dvb-frontends/sp887x.c
+++ b/drivers/media/dvb-frontends/sp887x.c
@@ -229,7 +229,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->hierarchy) {
         case HIERARCHY_NONE:
@@ -248,7 +248,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->code_rate_HP) {
         case FEC_1_2:
@@ -270,7 +270,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         if (known_parameters)
                 *reg0xc05 |= (2 << 1);  /* use specified parameters */
diff --git a/drivers/media/dvb-frontends/stb6100.c b/drivers/media/dvb-frontends/stb6100.c
index 2e93e65d2cdb..45f9523f968f 100644
--- a/drivers/media/dvb-frontends/stb6100.c
+++ b/drivers/media/dvb-frontends/stb6100.c
@@ -575,8 +575,8 @@ struct dvb_frontend *stb6100_attach(struct dvb_frontend *fe,
         struct stb6100_state *state = NULL;
 
         state = kzalloc(sizeof (struct stb6100_state), GFP_KERNEL);
-        if (state == NULL)
-                goto error;
+        if (!state)
+                return NULL;
 
         state->config           = config;
         state->i2c              = i2c;
@@ -587,10 +587,6 @@ struct dvb_frontend *stb6100_attach(struct dvb_frontend *fe,
 
         printk("%s: Attaching STB6100 \n", __func__);
         return fe;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static int stb6100_release(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/stv0299.c b/drivers/media/dvb-frontends/stv0299.c
index 057b5f8effc0..92a6075cd82f 100644
--- a/drivers/media/dvb-frontends/stv0299.c
+++ b/drivers/media/dvb-frontends/stv0299.c
@@ -199,7 +199,7 @@ static int stv0299_wait_diseqc_fifo (struct stv0299_state* state, int timeout)
                         return -ETIMEDOUT;
                 }
                 msleep(10);
-        };
+        }
 
         return 0;
 }
@@ -216,7 +216,7 @@ static int stv0299_wait_diseqc_idle (struct stv0299_state* state, int timeout)
                         return -ETIMEDOUT;
                 }
                 msleep(10);
-        };
+        }
 
         return 0;
 }
@@ -387,7 +387,7 @@ static int stv0299_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltag
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         if (state->config->op0_off)
                 reg0x0c &= ~0x10;
diff --git a/drivers/media/dvb-frontends/stv0900_core.c b/drivers/media/dvb-frontends/stv0900_core.c
index 7f1badaf0d03..262dfa503c2a 100644
--- a/drivers/media/dvb-frontends/stv0900_core.c
+++ b/drivers/media/dvb-frontends/stv0900_core.c
@@ -1552,8 +1552,8 @@ static int stv0900_status(struct stv0900_internal *intp,
                 bitrate = (stv0900_get_mclk_freq(intp, intp->quartz)/1000000)
                         * (tsbitrate1_val << 8 | tsbitrate0_val);
                 bitrate /= 16384;
-                dprintk("TS bitrate = %d Mbit/sec \n", bitrate);
-        };
+                dprintk("TS bitrate = %d Mbit/sec\n", bitrate);
+        }
 
         return locked;
 }
diff --git a/drivers/media/dvb-frontends/tda665x.c b/drivers/media/dvb-frontends/tda665x.c
index 2c1c759a4f42..63cc12378d9a 100644
--- a/drivers/media/dvb-frontends/tda665x.c
+++ b/drivers/media/dvb-frontends/tda665x.c
@@ -228,8 +228,8 @@ struct dvb_frontend *tda665x_attach(struct dvb_frontend *fe,
         struct dvb_tuner_info *info;
 
         state = kzalloc(sizeof(struct tda665x_state), GFP_KERNEL);
-        if (state == NULL)
-                goto exit;
+        if (!state)
+                return NULL;
 
         state->config           = config;
         state->i2c              = i2c;
@@ -246,10 +246,6 @@ struct dvb_frontend *tda665x_attach(struct dvb_frontend *fe,
         printk(KERN_DEBUG "%s: Attaching TDA665x (%s) tuner\n", __func__, info->name);
 
         return fe;
-
-exit:
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL(tda665x_attach);
 
diff --git a/drivers/media/dvb-frontends/tda8083.c b/drivers/media/dvb-frontends/tda8083.c
index 15912c96926a..9d08350fe4b0 100644
--- a/drivers/media/dvb-frontends/tda8083.c
+++ b/drivers/media/dvb-frontends/tda8083.c
@@ -175,7 +175,7 @@ static void tda8083_wait_diseqc_fifo (struct tda8083_state* state, int timeout)
                !(tda8083_readreg(state, 0x02) & 0x80))
         {
                 msleep(50);
-        };
+        }
 }
 
 static int tda8083_set_tone (struct tda8083_state* state, fe_sec_tone_mode_t tone)
@@ -215,7 +215,7 @@ static int tda8083_send_diseqc_burst (struct tda8083_state* state, fe_sec_mini_c
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         tda8083_wait_diseqc_fifo (state, 100);
 
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index d8eac3e30a7e..2cee69e34184 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -599,7 +599,7 @@ static void cx23885_initialize(struct i2c_client *client)
                 cx25840_write4(client, 0x114, 0x01bf0c9e);
                 cx25840_write4(client, 0x110, 0x000a030c);
                 break;
-        };
+        }
 
         /* ADC2 input select */
         cx25840_write(client, 0x102, 0x10);
diff --git a/drivers/media/i2c/m5mols/m5mols.h b/drivers/media/i2c/m5mols/m5mols.h
index 86c815be348c..90a6c520f115 100644
--- a/drivers/media/i2c/m5mols/m5mols.h
+++ b/drivers/media/i2c/m5mols/m5mols.h
@@ -16,9 +16,17 @@
 #ifndef M5MOLS_H
 #define M5MOLS_H
 
+#include <linux/sizes.h>
 #include <media/v4l2-subdev.h>
 #include "m5mols_reg.h"
 
+
+/* An amount of data transmitted in addition to the value
+ * determined by CAPP_JPEG_SIZE_MAX register.
+ */
+#define M5MOLS_JPEG_TAGS_SIZE           0x20000
+#define M5MOLS_MAIN_JPEG_SIZE_MAX       (5 * SZ_1M)
+
 extern int m5mols_debug;
 
 enum m5mols_restype {
@@ -67,12 +75,14 @@ struct m5mols_exif {
 /**
  * struct m5mols_capture - Structure for the capture capability
  * @exif: EXIF information
+ * @buf_size: internal JPEG frame buffer size, in bytes
  * @main: size in bytes of the main image
  * @thumb: size in bytes of the thumb image, if it was accompanied
  * @total: total size in bytes of the produced image
  */
 struct m5mols_capture {
         struct m5mols_exif exif;
+        unsigned int buf_size;
         u32 main;
         u32 thumb;
         u32 total;
diff --git a/drivers/media/i2c/m5mols/m5mols_capture.c b/drivers/media/i2c/m5mols/m5mols_capture.c
index cb243bd278ce..ab34ccedf31e 100644
--- a/drivers/media/i2c/m5mols/m5mols_capture.c
+++ b/drivers/media/i2c/m5mols/m5mols_capture.c
@@ -105,6 +105,7 @@ static int m5mols_capture_info(struct m5mols_info *info)
 
 int m5mols_start_capture(struct m5mols_info *info)
 {
+        unsigned int framesize = info->cap.buf_size - M5MOLS_JPEG_TAGS_SIZE;
         struct v4l2_subdev *sd = &info->sd;
         int ret;
 
@@ -121,6 +122,8 @@ int m5mols_start_capture(struct m5mols_info *info)
         if (!ret)
                 ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, info->resolution);
         if (!ret)
+                ret = m5mols_write(sd, CAPP_JPEG_SIZE_MAX, framesize);
+        if (!ret)
                 ret = m5mols_set_mode(info, REG_CAPTURE);
         if (!ret)
                 /* Wait until a frame is captured to ISP internal memory */
diff --git a/drivers/media/i2c/m5mols/m5mols_core.c b/drivers/media/i2c/m5mols/m5mols_core.c
index 2f490ef26c38..8131d651de9e 100644
--- a/drivers/media/i2c/m5mols/m5mols_core.c
+++ b/drivers/media/i2c/m5mols/m5mols_core.c
@@ -599,6 +599,51 @@ static int m5mols_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
         return ret;
 }
 
+static int m5mols_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+                                 struct v4l2_mbus_frame_desc *fd)
+{
+        struct m5mols_info *info = to_m5mols(sd);
+
+        if (pad != 0 || fd == NULL)
+                return -EINVAL;
+
+        mutex_lock(&info->lock);
+        /*
+         * .get_frame_desc is only used for compressed formats,
+         * thus we always return the capture frame parameters here.
+         */
+        fd->entry[0].length = info->cap.buf_size;
+        fd->entry[0].pixelcode = info->ffmt[M5MOLS_RESTYPE_CAPTURE].code;
+        mutex_unlock(&info->lock);
+
+        fd->entry[0].flags = V4L2_MBUS_FRAME_DESC_FL_LEN_MAX;
+        fd->num_entries = 1;
+
+        return 0;
+}
+
+static int m5mols_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+                                 struct v4l2_mbus_frame_desc *fd)
+{
+        struct m5mols_info *info = to_m5mols(sd);
+        struct v4l2_mbus_framefmt *mf = &info->ffmt[M5MOLS_RESTYPE_CAPTURE];
+
+        if (pad != 0 || fd == NULL)
+                return -EINVAL;
+
+        fd->entry[0].flags = V4L2_MBUS_FRAME_DESC_FL_LEN_MAX;
+        fd->num_entries = 1;
+        fd->entry[0].length = clamp_t(u32, fd->entry[0].length,
+                                      mf->width * mf->height,
+                                      M5MOLS_MAIN_JPEG_SIZE_MAX);
+        mutex_lock(&info->lock);
+        info->cap.buf_size = fd->entry[0].length;
+        mutex_unlock(&info->lock);
+
+        return 0;
+}
+
+
 static int m5mols_enum_mbus_code(struct v4l2_subdev *sd,
                                  struct v4l2_subdev_fh *fh,
                                  struct v4l2_subdev_mbus_code_enum *code)
@@ -615,6 +660,8 @@ static struct v4l2_subdev_pad_ops m5mols_pad_ops = {
         .enum_mbus_code = m5mols_enum_mbus_code,
         .get_fmt        = m5mols_get_fmt,
         .set_fmt        = m5mols_set_fmt,
+        .get_frame_desc = m5mols_get_frame_desc,
+        .set_frame_desc = m5mols_set_frame_desc,
 };
 
 /**
diff --git a/drivers/media/i2c/m5mols/m5mols_reg.h b/drivers/media/i2c/m5mols/m5mols_reg.h
index 14d4be72aeff..58d8027508df 100644
--- a/drivers/media/i2c/m5mols/m5mols_reg.h
+++ b/drivers/media/i2c/m5mols/m5mols_reg.h
@@ -310,6 +310,7 @@
 #define REG_JPEG                0x10
 
 #define CAPP_MAIN_IMAGE_SIZE    I2C_REG(CAT_CAPT_PARM, 0x01, 1)
+#define CAPP_JPEG_SIZE_MAX      I2C_REG(CAT_CAPT_PARM, 0x0f, 4)
 #define CAPP_JPEG_RATIO         I2C_REG(CAT_CAPT_PARM, 0x17, 1)
 
 #define CAPP_MCC_MODE           I2C_REG(CAT_CAPT_PARM, 0x1d, 1)
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index 2c0f4077c491..e32833262d32 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -574,7 +574,6 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN           (V4L2_CID_USER_BASE | 0x1001)
 #define V4L2_CID_BLC_AUTO               (V4L2_CID_USER_BASE | 0x1002)
 #define V4L2_CID_BLC_TARGET_LEVEL       (V4L2_CID_USER_BASE | 0x1003)
 #define V4L2_CID_BLC_ANALOG_OFFSET      (V4L2_CID_USER_BASE | 0x1004)
@@ -740,18 +739,6 @@ static const char * const mt9p031_test_pattern_menu[] = {
 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
         {
                 .ops            = &mt9p031_ctrl_ops,
-                .id             = V4L2_CID_TEST_PATTERN,
-                .type           = V4L2_CTRL_TYPE_MENU,
-                .name           = "Test Pattern",
-                .min            = 0,
-                .max            = ARRAY_SIZE(mt9p031_test_pattern_menu) - 1,
-                .step           = 0,
-                .def            = 0,
-                .flags          = 0,
-                .menu_skip_mask = 0,
-                .qmenu          = mt9p031_test_pattern_menu,
-        }, {
-                .ops            = &mt9p031_ctrl_ops,
                 .id             = V4L2_CID_BLC_AUTO,
                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
                 .name           = "BLC, Auto",
@@ -950,7 +937,7 @@ static int mt9p031_probe(struct i2c_client *client,
         mt9p031->model = did->driver_data;
         mt9p031->reset = -1;
 
-        v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 5);
+        v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
 
         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
                           V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
@@ -966,6 +953,10 @@ static int mt9p031_probe(struct i2c_client *client,
         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
                           V4L2_CID_PIXEL_RATE, pdata->target_freq,
                           pdata->target_freq, 1, pdata->target_freq);
+        v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+                          V4L2_CID_TEST_PATTERN,
+                          ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
+                          0, mt9p031_test_pattern_menu);
 
         for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
                 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
diff --git a/drivers/media/i2c/mt9t001.c b/drivers/media/i2c/mt9t001.c
index 6d343adf891d..2e189d8b71bb 100644
--- a/drivers/media/i2c/mt9t001.c
+++ b/drivers/media/i2c/mt9t001.c
@@ -371,7 +371,7 @@ static int mt9t001_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN           (V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_TEST_PATTERN_COLOR     (V4L2_CID_USER_BASE | 0x1001)
 #define V4L2_CID_BLACK_LEVEL_AUTO       (V4L2_CID_USER_BASE | 0x1002)
 #define V4L2_CID_BLACK_LEVEL_OFFSET     (V4L2_CID_USER_BASE | 0x1003)
 #define V4L2_CID_BLACK_LEVEL_CALIBRATE  (V4L2_CID_USER_BASE | 0x1004)
@@ -487,12 +487,11 @@ static int mt9t001_s_ctrl(struct v4l2_ctrl *ctrl)
                                      ctrl->val >> 16);
 
         case V4L2_CID_TEST_PATTERN:
-                ret = mt9t001_set_output_control(mt9t001,
+                return mt9t001_set_output_control(mt9t001,
                         ctrl->val ? 0 : MT9T001_OUTPUT_CONTROL_TEST_DATA,
                         ctrl->val ? MT9T001_OUTPUT_CONTROL_TEST_DATA : 0);
-                if (ret < 0)
-                        return ret;
 
+        case V4L2_CID_TEST_PATTERN_COLOR:
                 return mt9t001_write(client, MT9T001_TEST_DATA, ctrl->val << 2);
 
         case V4L2_CID_BLACK_LEVEL_AUTO:
@@ -533,12 +532,17 @@ static struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
         .s_ctrl = mt9t001_s_ctrl,
 };
 
+static const char * const mt9t001_test_pattern_menu[] = {
+        "Disabled",
+        "Enabled",
+};
+
 static const struct v4l2_ctrl_config mt9t001_ctrls[] = {
         {
                 .ops            = &mt9t001_ctrl_ops,
-                .id             = V4L2_CID_TEST_PATTERN,
+                .id             = V4L2_CID_TEST_PATTERN_COLOR,
                 .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Test pattern",
+                .name           = "Test Pattern Color",
                 .min            = 0,
                 .max            = 1023,
                 .step           = 1,
@@ -741,7 +745,7 @@ static int mt9t001_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_ctrl_handler_init(&mt9t001->ctrls, ARRAY_SIZE(mt9t001_ctrls) +
-                                                ARRAY_SIZE(mt9t001_gains) + 3);
+                                                ARRAY_SIZE(mt9t001_gains) + 4);
 
         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
                           V4L2_CID_EXPOSURE, MT9T001_SHUTTER_WIDTH_MIN,
@@ -752,6 +756,10 @@ static int mt9t001_probe(struct i2c_client *client,
         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
                           V4L2_CID_PIXEL_RATE, pdata->ext_clk, pdata->ext_clk,
                           1, pdata->ext_clk);
+        v4l2_ctrl_new_std_menu_items(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+                        V4L2_CID_TEST_PATTERN,
+                        ARRAY_SIZE(mt9t001_test_pattern_menu) - 1, 0,
+                        0, mt9t001_test_pattern_menu);
 
         for (i = 0; i < ARRAY_SIZE(mt9t001_ctrls); ++i)
                 v4l2_ctrl_new_custom(&mt9t001->ctrls, &mt9t001_ctrls[i], NULL);
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
index e2177405dad2..3f356cb28256 100644
--- a/drivers/media/i2c/mt9v032.c
+++ b/drivers/media/i2c/mt9v032.c
@@ -141,6 +141,10 @@ struct mt9v032 {
         u16 chip_control;
         u16 aec_agc;
         u16 hblank;
+        struct {
+                struct v4l2_ctrl *test_pattern;
+                struct v4l2_ctrl *test_pattern_color;
+        };
 };
 
 static struct mt9v032 *to_mt9v032(struct v4l2_subdev *sd)
@@ -500,7 +504,7 @@ static int mt9v032_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN           (V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_TEST_PATTERN_COLOR     (V4L2_CID_USER_BASE | 0x1001)
 
 static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 {
@@ -545,7 +549,7 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
                 break;
 
         case V4L2_CID_TEST_PATTERN:
-                switch (ctrl->val) {
+                switch (mt9v032->test_pattern->val) {
                 case 0:
                         data = 0;
                         break;
@@ -562,13 +566,13 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
                              | MT9V032_TEST_PATTERN_ENABLE;
                         break;
                 default:
-                        data = (ctrl->val << MT9V032_TEST_PATTERN_DATA_SHIFT)
+                        data = (mt9v032->test_pattern_color->val <<
+                                MT9V032_TEST_PATTERN_DATA_SHIFT)
                              | MT9V032_TEST_PATTERN_USE_DATA
                              | MT9V032_TEST_PATTERN_ENABLE
                              | MT9V032_TEST_PATTERN_FLIP;
                         break;
                 }
-
                 return mt9v032_write(client, MT9V032_TEST_PATTERN, data);
         }
 
@@ -579,18 +583,24 @@ static struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
         .s_ctrl = mt9v032_s_ctrl,
 };
 
-static const struct v4l2_ctrl_config mt9v032_ctrls[] = {
-        {
-                .ops            = &mt9v032_ctrl_ops,
-                .id             = V4L2_CID_TEST_PATTERN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Test pattern",
-                .min            = 0,
-                .max            = 1023,
-                .step           = 1,
-                .def            = 0,
-                .flags          = 0,
-        }
+static const char * const mt9v032_test_pattern_menu[] = {
+        "Disabled",
+        "Gray Vertical Shade",
+        "Gray Horizontal Shade",
+        "Gray Diagonal Shade",
+        "Plain",
+};
+
+static const struct v4l2_ctrl_config mt9v032_test_pattern_color = {
+        .ops            = &mt9v032_ctrl_ops,
+        .id             = V4L2_CID_TEST_PATTERN_COLOR,
+        .type           = V4L2_CTRL_TYPE_INTEGER,
+        .name           = "Test Pattern Color",
+        .min            = 0,
+        .max            = 1023,
+        .step           = 1,
+        .def            = 0,
+        .flags          = 0,
 };
 
 /* -----------------------------------------------------------------------------
@@ -741,7 +751,7 @@ static int mt9v032_probe(struct i2c_client *client,
         mutex_init(&mt9v032->power_lock);
         mt9v032->pdata = pdata;
 
-        v4l2_ctrl_handler_init(&mt9v032->ctrls, ARRAY_SIZE(mt9v032_ctrls) + 8);
+        v4l2_ctrl_handler_init(&mt9v032->ctrls, 10);
 
         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
                           V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
@@ -763,6 +773,14 @@ static int mt9v032_probe(struct i2c_client *client,
                           V4L2_CID_VBLANK, MT9V032_VERTICAL_BLANKING_MIN,
                           MT9V032_VERTICAL_BLANKING_MAX, 1,
                           MT9V032_VERTICAL_BLANKING_DEF);
+        mt9v032->test_pattern = v4l2_ctrl_new_std_menu_items(&mt9v032->ctrls,
+                                &mt9v032_ctrl_ops, V4L2_CID_TEST_PATTERN,
+                                ARRAY_SIZE(mt9v032_test_pattern_menu) - 1, 0, 0,
+                                mt9v032_test_pattern_menu);
+        mt9v032->test_pattern_color = v4l2_ctrl_new_custom(&mt9v032->ctrls,
+                                      &mt9v032_test_pattern_color, NULL);
+
+        v4l2_ctrl_cluster(2, &mt9v032->test_pattern);
 
         mt9v032->pixel_rate =
                 v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
@@ -784,8 +802,6 @@ static int mt9v032_probe(struct i2c_client *client,
                 v4l2_ctrl_cluster(2, &mt9v032->link_freq);
         }
 
-        for (i = 0; i < ARRAY_SIZE(mt9v032_ctrls); ++i)
-                v4l2_ctrl_new_custom(&mt9v032->ctrls, &mt9v032_ctrls[i], NULL);
 
         mt9v032->subdev.ctrl_handler = &mt9v032->ctrls;
 
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index 78ac5744cb5d..d2d298b6354e 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -684,6 +684,11 @@ static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
                 &container_of(ctrl->handler, struct ov2640_priv, hdl)->subdev;
         struct i2c_client  *client = v4l2_get_subdevdata(sd);
         u8 val;
+        int ret;
+
+        ret = i2c_smbus_write_byte_data(client, BANK_SEL, BANK_SEL_SENS);
+        if (ret < 0)
+                return ret;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index e5c0eedebc58..c31cc04fffd2 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -28,6 +28,18 @@
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
 
+#define THS7303_CHANNEL_1       1
+#define THS7303_CHANNEL_2       2
+#define THS7303_CHANNEL_3       3
+
+enum ths7303_filter_mode {
+        THS7303_FILTER_MODE_480I_576I,
+        THS7303_FILTER_MODE_480P_576P,
+        THS7303_FILTER_MODE_720P_1080I,
+        THS7303_FILTER_MODE_1080P,
+        THS7303_FILTER_MODE_DISABLE
+};
+
 MODULE_DESCRIPTION("TI THS7303 video amplifier driver");
 MODULE_AUTHOR("Chaithrika U S");
 MODULE_LICENSE("GPL");
@@ -37,35 +49,96 @@ module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Debug level 0-1");
 
 /* following function is used to set ths7303 */
-static int ths7303_setvalue(struct v4l2_subdev *sd, v4l2_std_id std)
+int ths7303_setval(struct v4l2_subdev *sd, enum ths7303_filter_mode mode)
 {
+        u8 input_bias_chroma = 3;
+        u8 input_bias_luma = 3;
+        int disable = 0;
         int err = 0;
-        u8 val;
-        struct i2c_client *client;
+        u8 val = 0;
+        u8 temp;
 
-        client = v4l2_get_subdevdata(sd);
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-        if (std & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
-                val = 0x02;
-                v4l2_dbg(1, debug, sd, "setting value for SDTV format\n");
-        } else {
-                val = 0x00;
-                v4l2_dbg(1, debug, sd, "disabling all channels\n");
+        if (!client)
+                return -EINVAL;
+
+        switch (mode) {
+        case THS7303_FILTER_MODE_1080P:
+                val = (3 << 6);
+                val |= (3 << 3);
+                break;
+        case THS7303_FILTER_MODE_720P_1080I:
+                val = (2 << 6);
+                val |= (2 << 3);
+                break;
+        case THS7303_FILTER_MODE_480P_576P:
+                val = (1 << 6);
+                val |= (1 << 3);
+                break;
+        case THS7303_FILTER_MODE_480I_576I:
+                break;
+        case THS7303_FILTER_MODE_DISABLE:
+                pr_info("mode disabled\n");
+                /* disable all channels */
+                disable = 1;
+        default:
+                /* disable all channels */
+                disable = 1;
         }
+        /* Setup channel 2 - Luma - Green */
+        temp = val;
+        if (!disable)
+                val |= input_bias_luma;
+        err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_2, val);
+        if (err)
+                goto out;
 
-        err |= i2c_smbus_write_byte_data(client, 0x01, val);
-        err |= i2c_smbus_write_byte_data(client, 0x02, val);
-        err |= i2c_smbus_write_byte_data(client, 0x03, val);
+        /* setup two chroma channels */
+        if (!disable)
+                temp |= input_bias_chroma;
 
+        err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_1, temp);
         if (err)
-                v4l2_err(sd, "write failed\n");
+                goto out;
 
+        err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_3, temp);
+        if (err)
+                goto out;
+        return err;
+out:
+        pr_info("write byte data failed\n");
         return err;
 }
 
 static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
-        return ths7303_setvalue(sd, norm);
+        if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM))
+                return ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
+        else
+                return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+}
+
+/* for setting filter for HD output */
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
+                               struct v4l2_dv_timings *dv_timings)
+{
+        u32 height = dv_timings->bt.height;
+        int interlaced = dv_timings->bt.interlaced;
+        int res = 0;
+
+        if (height == 1080 && !interlaced)
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_1080P);
+        else if ((height == 720 && !interlaced) ||
+                        (height == 1080 && interlaced))
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_720P_1080I);
+        else if ((height == 480 || height == 576) && !interlaced)
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_480P_576P);
+        else
+                /* disable all channels */
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+
+        return res;
 }
 
 static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
@@ -78,6 +151,7 @@ static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_video_ops ths7303_video_ops = {
         .s_std_output   = ths7303_s_std_output,
+        .s_dv_timings    = ths7303_s_dv_timings,
 };
 
 static const struct v4l2_subdev_core_ops ths7303_core_ops = {
@@ -107,7 +181,7 @@ static int ths7303_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
 
-        return ths7303_setvalue(sd, std_id);
+        return ths7303_s_std_output(sd, std_id);
 }
 
 static int ths7303_remove(struct i2c_client *client)
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index 1f3943bb87d5..d5e10215a28f 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -519,6 +519,12 @@ static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
 
         *std_id = V4L2_STD_UNKNOWN;
 
+        /* To query the standard the TVP514x must power on the ADCs. */
+        if (!decoder->streaming) {
+                tvp514x_s_stream(sd, 1);
+                msleep(LOCK_RETRY_DELAY);
+        }
+
         /* query the current standard */
         current_std = tvp514x_query_current_std(sd);
         if (current_std == STD_INVALID)
@@ -625,25 +631,12 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
         int err;
         enum tvp514x_input input_sel;
         enum tvp514x_output output_sel;
-        u8 sync_lock_status, lock_mask;
-        int try_count = LOCK_RETRY_COUNT;
 
         if ((input >= INPUT_INVALID) ||
                         (output >= OUTPUT_INVALID))
                 /* Index out of bound */
                 return -EINVAL;
 
-        /*
-         * For the sequence streamon -> streamoff and again s_input
-         * it fails to lock the signal, since streamoff puts TVP514x
-         * into power off state which leads to failure in sub-sequent s_input.
-         *
-         * So power up the TVP514x device here, since it is important to lock
-         * the signal at this stage.
-         */
-        if (!decoder->streaming)
-                tvp514x_s_stream(sd, 1);
-
         input_sel = input;
         output_sel = output;
 
@@ -660,64 +653,6 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
 
         decoder->tvp514x_regs[REG_INPUT_SEL].val = input_sel;
         decoder->tvp514x_regs[REG_OUTPUT_FORMATTER1].val = output_sel;
-
-        /* Clear status */
-        msleep(LOCK_RETRY_DELAY);
-        err =
-            tvp514x_write_reg(sd, REG_CLEAR_LOST_LOCK, 0x01);
-        if (err)
-                return err;
-
-        switch (input_sel) {
-        case INPUT_CVBS_VI1A:
-        case INPUT_CVBS_VI1B:
-        case INPUT_CVBS_VI1C:
-        case INPUT_CVBS_VI2A:
-        case INPUT_CVBS_VI2B:
-        case INPUT_CVBS_VI2C:
-        case INPUT_CVBS_VI3A:
-        case INPUT_CVBS_VI3B:
-        case INPUT_CVBS_VI3C:
-        case INPUT_CVBS_VI4A:
-                lock_mask = STATUS_CLR_SUBCAR_LOCK_BIT |
-                        STATUS_HORZ_SYNC_LOCK_BIT |
-                        STATUS_VIRT_SYNC_LOCK_BIT;
-                break;
-
-        case INPUT_SVIDEO_VI2A_VI1A:
-        case INPUT_SVIDEO_VI2B_VI1B:
-        case INPUT_SVIDEO_VI2C_VI1C:
-        case INPUT_SVIDEO_VI2A_VI3A:
-        case INPUT_SVIDEO_VI2B_VI3B:
-        case INPUT_SVIDEO_VI2C_VI3C:
-        case INPUT_SVIDEO_VI4A_VI1A:
-        case INPUT_SVIDEO_VI4A_VI1B:
-        case INPUT_SVIDEO_VI4A_VI1C:
-        case INPUT_SVIDEO_VI4A_VI3A:
-        case INPUT_SVIDEO_VI4A_VI3B:
-        case INPUT_SVIDEO_VI4A_VI3C:
-                lock_mask = STATUS_HORZ_SYNC_LOCK_BIT |
-                        STATUS_VIRT_SYNC_LOCK_BIT;
-                break;
-        /* Need to add other interfaces*/
-        default:
-                return -EINVAL;
-        }
-
-        while (try_count-- > 0) {
-                /* Allow decoder to sync up with new input */
-                msleep(LOCK_RETRY_DELAY);
-
-                sync_lock_status = tvp514x_read_reg(sd,
-                                REG_STATUS1);
-                if (lock_mask == (sync_lock_status & lock_mask))
-                        /* Input detected */
-                        break;
-        }
-
-        if (try_count < 0)
-                return -EINVAL;
-
         decoder->input = input;
         decoder->output = output;
 
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index b68918c97f66..56c6c77793d7 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -668,6 +668,12 @@ static const struct v4l2_queryctrl bttv_ctls[] = {
                 .default_value = 32768,
                 .type          = V4L2_CTRL_TYPE_INTEGER,
         },{
+                .id            = V4L2_CID_COLOR_KILLER,
+                .name          = "Color killer",
+                .minimum       = 0,
+                .maximum       = 1,
+                .type          = V4L2_CTRL_TYPE_BOOLEAN,
+        }, {
                 .id            = V4L2_CID_HUE,
                 .name          = "Hue",
                 .minimum       = 0,
@@ -1474,6 +1480,9 @@ static int bttv_g_ctrl(struct file *file, void *priv,
         case V4L2_CID_SATURATION:
                 c->value = btv->saturation;
                 break;
+        case V4L2_CID_COLOR_KILLER:
+                c->value = btv->opt_color_killer;
+                break;
 
         case V4L2_CID_AUDIO_MUTE:
         case V4L2_CID_AUDIO_VOLUME:
@@ -1526,7 +1535,6 @@ static int bttv_s_ctrl(struct file *file, void *f,
                                         struct v4l2_control *c)
 {
         int err;
-        int val;
         struct bttv_fh *fh = f;
         struct bttv *btv = fh->btv;
 
@@ -1547,6 +1555,16 @@ static int bttv_s_ctrl(struct file *file, void *f,
         case V4L2_CID_SATURATION:
                 bt848_sat(btv, c->value);
                 break;
+        case V4L2_CID_COLOR_KILLER:
+                btv->opt_color_killer = c->value;
+                if (btv->opt_color_killer) {
+                        btor(BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
+                        btor(BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
+                } else {
+                        btand(~BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
+                        btand(~BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
+                }
+                break;
         case V4L2_CID_AUDIO_MUTE:
                 audio_mute(btv, c->value);
                 /* fall through */
@@ -1564,9 +1582,13 @@ static int bttv_s_ctrl(struct file *file, void *f,
 
         case V4L2_CID_PRIVATE_CHROMA_AGC:
                 btv->opt_chroma_agc = c->value;
-                val = btv->opt_chroma_agc ? BT848_SCLOOP_CAGC : 0;
-                btwrite(val, BT848_E_SCLOOP);
-                btwrite(val, BT848_O_SCLOOP);
+                if (btv->opt_chroma_agc) {
+                        btor(BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
+                        btor(BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
+                } else {
+                        btand(~BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
+                        btand(~BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
+                }
                 break;
         case V4L2_CID_PRIVATE_COMBFILTER:
                 btv->opt_combfilter = c->value;
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index 70fd4f23f605..9ec0adba236c 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -429,6 +429,7 @@ struct bttv {
         int opt_lumafilter;
         int opt_automute;
         int opt_chroma_agc;
+        int opt_color_killer;
         int opt_adc_crush;
         int opt_vcr_hack;
         int opt_whitecrush_upper;
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index ee3884fbc9ce..7d96fab7d246 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -646,7 +646,7 @@ static long dst_ca_ioctl(struct file *file, unsigned int cmd, unsigned long ioct
                 dprintk(verbose, DST_CA_INFO, 1, " -->CA_SET_PID Success !");
         default:
                 result = -EOPNOTSUPP;
-        };
+        }
  free_mem_and_exit:
         kfree (p_ca_message);
         kfree (p_ca_slot_info);
diff --git a/drivers/media/pci/cx23885/altera-ci.c b/drivers/media/pci/cx23885/altera-ci.c
index aee7f0dacff1..495781ee4711 100644
--- a/drivers/media/pci/cx23885/altera-ci.c
+++ b/drivers/media/pci/cx23885/altera-ci.c
@@ -416,7 +416,7 @@ static void netup_read_ci_status(struct work_struct *work)
                                 DVB_CA_EN50221_POLL_CAM_READY : 0);
                 ci_dbg_print("%s: setting CI[1] status = 0x%x\n",
                                 __func__, inter->state[1]->status);
-        };
+        }
 
         if (inter->state[0] != NULL) {
                 inter->state[0]->status =
@@ -425,7 +425,7 @@ static void netup_read_ci_status(struct work_struct *work)
                                 DVB_CA_EN50221_POLL_CAM_READY : 0);
                 ci_dbg_print("%s: setting CI[0] status = 0x%x\n",
                                 __func__, inter->state[0]->status);
-        };
+        }
 }
 
 /* CI irq handler */
diff --git a/drivers/media/pci/cx23885/cimax2.c b/drivers/media/pci/cx23885/cimax2.c
index c9f15d6dec40..6617774a326a 100644
--- a/drivers/media/pci/cx23885/cimax2.c
+++ b/drivers/media/pci/cx23885/cimax2.c
@@ -193,7 +193,7 @@ int netup_ci_op_cam(struct dvb_ca_en50221 *en50221, int slot,
                                 0, &store, 1);
                 if (ret != 0)
                         return ret;
-        };
+        }
         state->current_ci_flag = flag;
 
         mutex_lock(&dev->gpio_lock);
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 39a4a4b9ed7e..5acdf954ff6b 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -542,11 +542,13 @@ struct cx23885_board cx23885_boards[] = {
                         {
                                 .type   = CX23885_VMUX_COMPOSITE1,
                                 .vmux   = CX25840_COMPOSITE8,
+                                .amux   = CX25840_AUDIO7,
                         },
                         {
                                 .type   = CX23885_VMUX_SVIDEO,
                                 .vmux   = CX25840_SVIDEO_LUMA3 |
                                                 CX25840_SVIDEO_CHROMA4,
+                                .amux   = CX25840_AUDIO7,
                         },
                         {
                                 .type   = CX23885_VMUX_COMPONENT,
@@ -554,6 +556,7 @@ struct cx23885_board cx23885_boards[] = {
                                         CX25840_VIN1_CH1 |
                                         CX25840_VIN6_CH2 |
                                         CX25840_VIN7_CH3,
+                                .amux   = CX25840_AUDIO7,
                         },
                 },
         },
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 8c4a9a5f9a50..1a21926ca412 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -508,7 +508,8 @@ static int cx23885_video_mux(struct cx23885_dev *dev, unsigned int input)
                 (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1250) ||
                 (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255) ||
                 (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255_22111) ||
-                (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850)) {
+                (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850) ||
+                (dev->board == CX23885_BOARD_MYGICA_X8507)) {
                 /* Configure audio routing */
                 v4l2_subdev_call(dev->sd_cx25840, audio, s_routing,
                         INPUT(input)->amux, 0, 0);
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
index c8c94fbf5d8d..d33fc1a23030 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
@@ -761,7 +761,7 @@ int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev, int channel_select,
         }
 
         /* Default if filename is empty string */
-        if (strcmp(dev->input_filename_ch2, "") == 0) {
+        if (strcmp(dev->_filename_ch2, "") == 0) {
                 if (dev->_isNTSC_ch2) {
                         dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
                                 PIXEL_FRMT_411) ? "/root/vid411.yuv" :
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream.c b/drivers/media/pci/cx25821/cx25821-video-upstream.c
index 52c13e0b6492..6759fff8eb64 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream.c
@@ -808,7 +808,7 @@ int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev, int channel_select,
         }
 
         /* Default if filename is empty string */
-        if (strcmp(dev->input_filename, "") == 0) {
+        if (strcmp(dev->_filename, "") == 0) {
                 if (dev->_isNTSC) {
                         dev->_filename =
                                 (dev->_pixel_format == PIXEL_FRMT_411) ?
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index def363fb71c0..62184eb919e5 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -721,7 +721,7 @@ static int vidioc_g_fmt_vid_cap (struct file *file, void *priv,
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.bytesperline = 0;
-        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
         f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
         f->fmt.pix.width        = dev->width;
         f->fmt.pix.height       = dev->height;
@@ -739,7 +739,7 @@ static int vidioc_try_fmt_vid_cap (struct file *file, void *priv,
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.bytesperline = 0;
-        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
         f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
         dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
                 dev->width, dev->height, fh->mpegq.field );
@@ -755,7 +755,7 @@ static int vidioc_s_fmt_vid_cap (struct file *file, void *priv,
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.bytesperline = 0;
-        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
         f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
         dev->width              = f->fmt.pix.width;
         dev->height             = f->fmt.pix.height;
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index d803bba09525..666f83b2f3c0 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -896,7 +896,7 @@ static int samsung_smt_7020_set_voltage(struct dvb_frontend *fe,
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         return (i2c_transfer(&dev->core->i2c_adap, &msg, 1) == 1) ? 0 : -EIO;
 }
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index c04fb618e10b..d154bc197356 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -450,7 +450,7 @@ static irqreturn_t cx8802_irq(int irq, void *dev_id)
                         cx88_core_irq(core,status);
                 if (status & PCI_INT_TSINT)
                         cx8802_mpeg_irq(dev);
-        };
+        }
         if (MAX_IRQ_LOOP == loop) {
                 dprintk( 0, "clearing mask\n" );
                 printk(KERN_WARNING "%s/0: irq loop -- clearing mask\n",
diff --git a/drivers/media/pci/cx88/cx88-tvaudio.c b/drivers/media/pci/cx88/cx88-tvaudio.c
index 770ec05b5e9b..424fd97495dc 100644
--- a/drivers/media/pci/cx88/cx88-tvaudio.c
+++ b/drivers/media/pci/cx88/cx88-tvaudio.c
@@ -373,7 +373,7 @@ static void set_audio_standard_NICAM(struct cx88_core *core, u32 mode)
                 set_audio_registers(core, nicam_bgdki_common);
                 set_audio_registers(core, nicam_default);
                 break;
-        };
+        }
 
         mode |= EN_DMTRX_LR | EN_DMTRX_BYPASS;
         set_audio_finish(core, mode);
@@ -639,7 +639,7 @@ static void set_audio_standard_A2(struct cx88_core *core, u32 mode)
                 dprintk("%s Warning: wrong value\n", __func__);
                 return;
                 break;
-        };
+        }
 
         mode |= EN_FMRADIO_EN_RDS | EN_DMTRX_SUMDIFF;
         set_audio_finish(core, mode);
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index a146d50d7795..05171457bf28 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -1535,7 +1535,7 @@ static irqreturn_t cx8800_irq(int irq, void *dev_id)
                         cx88_core_irq(core,status);
                 if (status & PCI_INT_VIDINT)
                         cx8800_vid_irq(dev);
-        };
+        }
         if (10 == loop) {
                 printk(KERN_WARNING "%s/0: irq loop -- clearing mask\n",
                        core->name);
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 22f8758d047f..4a77124ee70e 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -1204,7 +1204,7 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
                 break;
         default:
                 /* nothing */;
-        };
+        }
         switch (c->id) {
         case V4L2_CID_BRIGHTNESS:
                 dev->ctl_bright = c->value;
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index f588d6296c76..181c7686e412 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -165,12 +165,12 @@ config VIDEO_SAMSUNG_S5P_JPEG
           This is a v4l2 driver for Samsung S5P and EXYNOS4 JPEG codec
 
 config VIDEO_SAMSUNG_S5P_MFC
-        tristate "Samsung S5P MFC 5.1 Video Codec"
+        tristate "Samsung S5P MFC Video Codec"
         depends on VIDEO_DEV && VIDEO_V4L2 && PLAT_S5P
         select VIDEOBUF2_DMA_CONTIG
         default n
         help
-            MFC 5.1 driver for V4L2.
+            MFC 5.1 and 6.x driver for V4L2
 
 config VIDEO_MX2_EMMAPRP
         tristate "MX2 eMMa-PrP support"
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
index c4a82a1a8a97..69d7a58c92c3 100644
--- a/drivers/media/platform/davinci/vpbe.c
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -174,26 +174,6 @@ static int vpbe_get_current_mode_info(struct vpbe_device *vpbe_dev,
         return 0;
 }
 
-static int vpbe_get_dv_preset_info(struct vpbe_device *vpbe_dev,
-                                   unsigned int dv_preset)
-{
-        struct vpbe_config *cfg = vpbe_dev->cfg;
-        struct vpbe_enc_mode_info var;
-        int curr_output = vpbe_dev->current_out_index;
-        int i;
-
-        for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
-                var = cfg->outputs[curr_output].modes[i];
-                if ((var.timings_type & VPBE_ENC_DV_PRESET) &&
-                  (var.timings.dv_preset == dv_preset)) {
-                        vpbe_dev->current_timings = var;
-                        return 0;
-                }
-        }
-
-        return -EINVAL;
-}
-
 /* Get std by std id */
 static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
                              v4l2_std_id std_id)
@@ -206,7 +186,7 @@ static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
         for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
                 var = cfg->outputs[curr_output].modes[i];
                 if ((var.timings_type & VPBE_ENC_STD) &&
-                  (var.timings.std_id & std_id)) {
+                  (var.std_id & std_id)) {
                         vpbe_dev->current_timings = var;
                         return 0;
                 }
@@ -344,38 +324,42 @@ static unsigned int vpbe_get_output(struct vpbe_device *vpbe_dev)
 }
 
 /**
- * vpbe_s_dv_preset - Set the given preset timings in the encoder
+ * vpbe_s_dv_timings - Set the given preset timings in the encoder
  *
- * Sets the preset if supported by the current encoder. Return the status.
+ * Sets the timings if supported by the current encoder. Return the status.
  * 0 - success & -EINVAL on error
  */
-static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
-                     struct v4l2_dv_preset *dv_preset)
+static int vpbe_s_dv_timings(struct vpbe_device *vpbe_dev,
+                    struct v4l2_dv_timings *dv_timings)
 {
         struct vpbe_config *cfg = vpbe_dev->cfg;
         int out_index = vpbe_dev->current_out_index;
+        struct vpbe_output *output = &cfg->outputs[out_index];
         int sd_index = vpbe_dev->current_sd_index;
-        int ret;
+        int ret, i;
 
 
         if (!(cfg->outputs[out_index].output.capabilities &
-            V4L2_OUT_CAP_PRESETS))
+            V4L2_OUT_CAP_DV_TIMINGS))
                 return -EINVAL;
 
-        ret = vpbe_get_dv_preset_info(vpbe_dev, dv_preset->preset);
-
-        if (ret)
-                return ret;
-
+        for (i = 0; i < output->num_modes; i++) {
+                if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS &&
+                    !memcmp(&output->modes[i].dv_timings,
+                                dv_timings, sizeof(*dv_timings)))
+                        break;
+        }
+        if (i >= output->num_modes)
+                return -EINVAL;
+        vpbe_dev->current_timings = output->modes[i];
         mutex_lock(&vpbe_dev->lock);
 
-
         ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
-                                        s_dv_preset, dv_preset);
+                                        s_dv_timings, dv_timings);
         if (!ret && (vpbe_dev->amp != NULL)) {
                 /* Call amplifier subdevice */
                 ret = v4l2_subdev_call(vpbe_dev->amp, video,
-                                s_dv_preset, dv_preset);
+                                s_dv_timings, dv_timings);
         }
         /* set the lcd controller output for the given mode */
         if (!ret) {
@@ -392,17 +376,17 @@ static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
 }
 
 /**
- * vpbe_g_dv_preset - Get the preset in the current encoder
+ * vpbe_g_dv_timings - Get the timings in the current encoder
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
-static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
-                     struct v4l2_dv_preset *dv_preset)
+static int vpbe_g_dv_timings(struct vpbe_device *vpbe_dev,
+                     struct v4l2_dv_timings *dv_timings)
 {
         if (vpbe_dev->current_timings.timings_type &
-          VPBE_ENC_DV_PRESET) {
-                dv_preset->preset = vpbe_dev->current_timings.timings.dv_preset;
+          VPBE_ENC_CUSTOM_TIMINGS) {
+                *dv_timings = vpbe_dev->current_timings.dv_timings;
                 return 0;
         }
 
@@ -410,13 +394,13 @@ static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
 }
 
 /**
- * vpbe_enum_dv_presets - Enumerate the dv presets in the current encoder
+ * vpbe_enum_dv_timings - Enumerate the dv timings in the current encoder
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
-static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
-                         struct v4l2_dv_enum_preset *preset_info)
+static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
+                         struct v4l2_enum_dv_timings *timings)
 {
         struct vpbe_config *cfg = vpbe_dev->cfg;
         int out_index = vpbe_dev->current_out_index;
@@ -424,12 +408,12 @@ static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
         int j = 0;
         int i;
 
-        if (!(output->output.capabilities & V4L2_OUT_CAP_PRESETS))
+        if (!(output->output.capabilities & V4L2_OUT_CAP_DV_TIMINGS))
                 return -EINVAL;
 
         for (i = 0; i < output->num_modes; i++) {
-                if (output->modes[i].timings_type == VPBE_ENC_DV_PRESET) {
-                        if (j == preset_info->index)
+                if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS) {
+                        if (j == timings->index)
                                 break;
                         j++;
                 }
@@ -437,9 +421,8 @@ static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
 
         if (i == output->num_modes)
                 return -EINVAL;
-
-        return v4l_fill_dv_preset_info(output->modes[i].timings.dv_preset,
-                                        preset_info);
+        timings->timings = output->modes[i].dv_timings;
+        return 0;
 }
 
 /**
@@ -489,10 +472,10 @@ static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
  */
 static int vpbe_g_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
 {
-        struct vpbe_enc_mode_info cur_timings = vpbe_dev->current_timings;
+        struct vpbe_enc_mode_info *cur_timings = &vpbe_dev->current_timings;
 
-        if (cur_timings.timings_type & VPBE_ENC_STD) {
-                *std_id = cur_timings.timings.std_id;
+        if (cur_timings->timings_type & VPBE_ENC_STD) {
+                *std_id = cur_timings->std_id;
                 return 0;
         }
 
@@ -511,7 +494,7 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
 {
         struct vpbe_enc_mode_info *preset_mode = NULL;
         struct vpbe_config *cfg = vpbe_dev->cfg;
-        struct v4l2_dv_preset dv_preset;
+        struct v4l2_dv_timings dv_timings;
         struct osd_state *osd_device;
         int out_index = vpbe_dev->current_out_index;
         int ret = 0;
@@ -530,11 +513,12 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
                          */
                         if (preset_mode->timings_type & VPBE_ENC_STD)
                                 return vpbe_s_std(vpbe_dev,
-                                                 &preset_mode->timings.std_id);
-                        if (preset_mode->timings_type & VPBE_ENC_DV_PRESET) {
-                                dv_preset.preset =
-                                        preset_mode->timings.dv_preset;
-                                return vpbe_s_dv_preset(vpbe_dev, &dv_preset);
+                                                 &preset_mode->std_id);
+                        if (preset_mode->timings_type &
+                                                VPBE_ENC_CUSTOM_TIMINGS) {
+                                dv_timings =
+                                        preset_mode->dv_timings;
+                                return vpbe_s_dv_timings(vpbe_dev, &dv_timings);
                         }
                 }
         }
@@ -626,11 +610,11 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                 vpbe_dev->dac_clk = clk_get(vpbe_dev->pdev, "vpss_dac");
                 if (IS_ERR(vpbe_dev->dac_clk)) {
                         ret =  PTR_ERR(vpbe_dev->dac_clk);
-                        goto vpbe_unlock;
+                        goto fail_mutex_unlock;
                 }
                 if (clk_enable(vpbe_dev->dac_clk)) {
                         ret =  -ENODEV;
-                        goto vpbe_unlock;
+                        goto fail_mutex_unlock;
                 }
         }
 
@@ -642,7 +626,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
         if (ret) {
                 v4l2_err(dev->driver,
                         "Unable to register v4l2 device.\n");
-                goto vpbe_fail_clock;
+                goto fail_clk_put;
         }
         v4l2_info(&vpbe_dev->v4l2_dev, "vpbe v4l2 device registered\n");
 
@@ -658,7 +642,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                 v4l2_err(&vpbe_dev->v4l2_dev,
                         "vpbe unable to init venc sub device\n");
                 ret = -ENODEV;
-                goto vpbe_fail_v4l2_device;
+                goto fail_dev_unregister;
         }
         /* initialize osd device */
         osd_device = vpbe_dev->osd_device;
@@ -669,7 +653,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                         v4l2_err(&vpbe_dev->v4l2_dev,
                                  "unable to initialize the OSD device");
                         err = -ENOMEM;
-                        goto vpbe_fail_v4l2_device;
+                        goto fail_dev_unregister;
                 }
         }
 
@@ -685,7 +669,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                 v4l2_err(&vpbe_dev->v4l2_dev,
                         "unable to allocate memory for encoders sub devices");
                 ret = -ENOMEM;
-                goto vpbe_fail_v4l2_device;
+                goto fail_dev_unregister;
         }
 
         i2c_adap = i2c_get_adapter(vpbe_dev->cfg->i2c_adapter_id);
@@ -711,7 +695,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                                          " failed to register",
                                          enc_info->module_name);
                                 ret = -ENODEV;
-                                goto vpbe_fail_sd_register;
+                                goto fail_kfree_encoders;
                         }
                 } else
                         v4l2_warn(&vpbe_dev->v4l2_dev, "non-i2c encoders"
@@ -730,7 +714,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                                          "amplifier %s failed to register",
                                          amp_info->module_name);
                                 ret = -ENODEV;
-                                goto vpbe_fail_amp_register;
+                                goto fail_kfree_encoders;
                         }
                         v4l2_info(&vpbe_dev->v4l2_dev,
                                           "v4l2 sub device %s registered\n",
@@ -770,16 +754,14 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
         /* TBD handling of bootargs for default output and mode */
         return 0;
 
-vpbe_fail_amp_register:
-        kfree(vpbe_dev->amp);
-vpbe_fail_sd_register:
+fail_kfree_encoders:
         kfree(vpbe_dev->encoders);
-vpbe_fail_v4l2_device:
+fail_dev_unregister:
         v4l2_device_unregister(&vpbe_dev->v4l2_dev);
-vpbe_fail_clock:
+fail_clk_put:
         if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0)
                 clk_put(vpbe_dev->dac_clk);
-vpbe_unlock:
+fail_mutex_unlock:
         mutex_unlock(&vpbe_dev->lock);
         return ret;
 }
@@ -810,9 +792,9 @@ static struct vpbe_device_ops vpbe_dev_ops = {
         .enum_outputs = vpbe_enum_outputs,
         .set_output = vpbe_set_output,
         .get_output = vpbe_get_output,
-        .s_dv_preset = vpbe_s_dv_preset,
-        .g_dv_preset = vpbe_g_dv_preset,
-        .enum_dv_presets = vpbe_enum_dv_presets,
+        .s_dv_timings = vpbe_s_dv_timings,
+        .g_dv_timings = vpbe_g_dv_timings,
+        .enum_dv_timings = vpbe_enum_dv_timings,
         .s_std = vpbe_s_std,
         .g_std = vpbe_g_std,
         .initialize = vpbe_initialize,
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 239f37bfa313..161c77650e2f 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -393,7 +393,7 @@ vpbe_disp_calculate_scale_factor(struct vpbe_display *disp_dev,
         int h_scale;
         int v_scale;
 
-        v4l2_std_id standard_id = vpbe_dev->current_timings.timings.std_id;
+        v4l2_std_id standard_id = vpbe_dev->current_timings.std_id;
 
         /*
          * Application initially set the image format. Current display
@@ -637,7 +637,7 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
         struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
         struct osd_layer_config *cfg = &layer->layer_info.config;
         struct osd_state *osd_device = disp_dev->osd_device;
-        struct v4l2_rect *rect = &crop->c;
+        struct v4l2_rect rect = crop->c;
         int ret;
 
         v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
@@ -648,21 +648,21 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
                 return -EINVAL;
         }
 
-        if (rect->top < 0)
-                rect->top = 0;
-        if (rect->left < 0)
-                rect->left = 0;
+        if (rect.top < 0)
+                rect.top = 0;
+        if (rect.left < 0)
+                rect.left = 0;
 
-        vpbe_disp_check_window_params(disp_dev, rect);
+        vpbe_disp_check_window_params(disp_dev, &rect);
 
         osd_device->ops.get_layer_config(osd_device,
                         layer->layer_info.id, cfg);
 
         vpbe_disp_calculate_scale_factor(disp_dev, layer,
-                                        rect->width,
-                                        rect->height);
-        vpbe_disp_adj_position(disp_dev, layer, rect->top,
-                                        rect->left);
+                                        rect.width,
+                                        rect.height);
+        vpbe_disp_adj_position(disp_dev, layer, rect.top,
+                                        rect.left);
         ret = osd_device->ops.set_layer_config(osd_device,
                                 layer->layer_info.id, cfg);
         if (ret < 0) {
@@ -943,7 +943,7 @@ static int vpbe_display_g_std(struct file *file, void *priv,
 
         /* Get the standard from the current encoder */
         if (vpbe_dev->current_timings.timings_type & VPBE_ENC_STD) {
-                *std_id = vpbe_dev->current_timings.timings.std_id;
+                *std_id = vpbe_dev->current_timings.std_id;
                 return 0;
         }
 
@@ -1029,29 +1029,29 @@ static int vpbe_display_g_output(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_enum_dv_presets - Enumerate the dv presets
+ * vpbe_display_enum_dv_timings - Enumerate the dv timings
  *
- * enum the preset in the current encoder. Return the status. 0 - success
+ * enum the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_enum_dv_presets(struct file *file, void *priv,
-                        struct v4l2_dv_enum_preset *preset)
+vpbe_display_enum_dv_timings(struct file *file, void *priv,
+                        struct v4l2_enum_dv_timings *timings)
 {
         struct vpbe_fh *fh = priv;
         struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
         int ret;
 
-        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_PRESETS\n");
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_TIMINGS\n");
 
         /* Enumerate outputs */
-        if (NULL == vpbe_dev->ops.enum_dv_presets)
+        if (NULL == vpbe_dev->ops.enum_dv_timings)
                 return -EINVAL;
 
-        ret = vpbe_dev->ops.enum_dv_presets(vpbe_dev, preset);
+        ret = vpbe_dev->ops.enum_dv_timings(vpbe_dev, timings);
         if (ret) {
                 v4l2_err(&vpbe_dev->v4l2_dev,
-                        "Failed to enumerate dv presets info\n");
+                        "Failed to enumerate dv timings info\n");
                 return -EINVAL;
         }
 
@@ -1059,21 +1059,21 @@ vpbe_display_enum_dv_presets(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_s_dv_preset - Set the dv presets
+ * vpbe_display_s_dv_timings - Set the dv timings
  *
- * Set the preset in the current encoder. Return the status. 0 - success
+ * Set the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_s_dv_preset(struct file *file, void *priv,
-                                struct v4l2_dv_preset *preset)
+vpbe_display_s_dv_timings(struct file *file, void *priv,
+                                struct v4l2_dv_timings *timings)
 {
         struct vpbe_fh *fh = priv;
         struct vpbe_layer *layer = fh->layer;
         struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
         int ret;
 
-        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_PRESETS\n");
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_TIMINGS\n");
 
 
         /* If streaming is started, return error */
@@ -1083,13 +1083,13 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
         }
 
         /* Set the given standard in the encoder */
-        if (!vpbe_dev->ops.s_dv_preset)
+        if (!vpbe_dev->ops.s_dv_timings)
                 return -EINVAL;
 
-        ret = vpbe_dev->ops.s_dv_preset(vpbe_dev, preset);
+        ret = vpbe_dev->ops.s_dv_timings(vpbe_dev, timings);
         if (ret) {
                 v4l2_err(&vpbe_dev->v4l2_dev,
-                        "Failed to set the dv presets info\n");
+                        "Failed to set the dv timings info\n");
                 return -EINVAL;
         }
         /* set the current norm to zero to be consistent. If STD is used
@@ -1101,26 +1101,25 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_g_dv_preset - Set the dv presets
+ * vpbe_display_g_dv_timings - Set the dv timings
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_g_dv_preset(struct file *file, void *priv,
-                                struct v4l2_dv_preset *dv_preset)
+vpbe_display_g_dv_timings(struct file *file, void *priv,
+                                struct v4l2_dv_timings *dv_timings)
 {
         struct vpbe_fh *fh = priv;
         struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
 
-        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_PRESETS\n");
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_TIMINGS\n");
 
         /* Get the given standard in the encoder */
 
         if (vpbe_dev->current_timings.timings_type &
-                                VPBE_ENC_DV_PRESET) {
-                dv_preset->preset =
-                        vpbe_dev->current_timings.timings.dv_preset;
+                                VPBE_ENC_CUSTOM_TIMINGS) {
+                *dv_timings = vpbe_dev->current_timings.dv_timings;
         } else {
                 return -EINVAL;
         }
@@ -1572,9 +1571,9 @@ static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
         .vidioc_enum_output      = vpbe_display_enum_output,
         .vidioc_s_output         = vpbe_display_s_output,
         .vidioc_g_output         = vpbe_display_g_output,
-        .vidioc_s_dv_preset      = vpbe_display_s_dv_preset,
-        .vidioc_g_dv_preset      = vpbe_display_g_dv_preset,
-        .vidioc_enum_dv_presets  = vpbe_display_enum_dv_presets,
+        .vidioc_s_dv_timings     = vpbe_display_s_dv_timings,
+        .vidioc_g_dv_timings     = vpbe_display_g_dv_timings,
+        .vidioc_enum_dv_timings  = vpbe_display_enum_dv_timings,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .vidioc_g_register       = vpbe_display_g_register,
         .vidioc_s_register       = vpbe_display_s_register,
@@ -1639,8 +1638,7 @@ static __devinit int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
                         VPBE_ENC_STD) {
                 vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
                 vbd->current_norm =
-                        disp_dev->vpbe_dev->
-                        current_timings.timings.std_id;
+                        disp_dev->vpbe_dev->current_timings.std_id;
         } else
                 vbd->current_norm = 0;
 
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
index 0302669622d6..aed7369b962a 100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -298,7 +298,7 @@ static int venc_set_480p59_94(struct v4l2_subdev *sd)
                 return -EINVAL;
 
         /* Setup clock at VPSS & VENC for SD */
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_480P59_94) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -345,7 +345,7 @@ static int venc_set_576p50(struct v4l2_subdev *sd)
           (pdata->venc_type != VPBE_VERSION_2))
                 return -EINVAL;
         /* Setup clock at VPSS & VENC for SD */
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_576P50) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -385,7 +385,7 @@ static int venc_set_720p60_internal(struct v4l2_subdev *sd)
         struct venc_state *venc = to_state(sd);
         struct venc_platform_data *pdata = venc->pdata;
 
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_720P60) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -413,7 +413,7 @@ static int venc_set_1080i30_internal(struct v4l2_subdev *sd)
         struct venc_state *venc = to_state(sd);
         struct venc_platform_data *pdata = venc->pdata;
 
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_1080P30) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -446,26 +446,27 @@ static int venc_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
         return -EINVAL;
 }
 
-static int venc_s_dv_preset(struct v4l2_subdev *sd,
-                            struct v4l2_dv_preset *dv_preset)
+static int venc_s_dv_timings(struct v4l2_subdev *sd,
+                            struct v4l2_dv_timings *dv_timings)
 {
         struct venc_state *venc = to_state(sd);
+        u32 height = dv_timings->bt.height;
         int ret;
 
-        v4l2_dbg(debug, 1, sd, "venc_s_dv_preset\n");
+        v4l2_dbg(debug, 1, sd, "venc_s_dv_timings\n");
 
-        if (dv_preset->preset == V4L2_DV_576P50)
+        if (height == 576)
                 return venc_set_576p50(sd);
-        else if (dv_preset->preset == V4L2_DV_480P59_94)
+        else if (height == 480)
                 return venc_set_480p59_94(sd);
-        else if ((dv_preset->preset == V4L2_DV_720P60) &&
+        else if ((height == 720) &&
                         (venc->pdata->venc_type == VPBE_VERSION_2)) {
                 /* TBD setup internal 720p mode here */
                 ret = venc_set_720p60_internal(sd);
                 /* for DM365 VPBE, there is DAC inside */
                 vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
                 return ret;
-        } else if ((dv_preset->preset == V4L2_DV_1080I30) &&
+        } else if ((height == 1080) &&
                 (venc->pdata->venc_type == VPBE_VERSION_2)) {
                 /* TBD setup internal 1080i mode here */
                 ret = venc_set_1080i30_internal(sd);
@@ -518,7 +519,7 @@ static const struct v4l2_subdev_core_ops venc_core_ops = {
 static const struct v4l2_subdev_video_ops venc_video_ops = {
         .s_routing = venc_s_routing,
         .s_std_output = venc_s_std_output,
-        .s_dv_preset = venc_s_dv_preset,
+        .s_dv_timings = venc_s_dv_timings,
 };
 
 static const struct v4l2_subdev_ops venc_ops = {
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 48052cbffc2b..8be492cd8ed4 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -1669,6 +1669,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
                              const struct v4l2_crop *crop)
 {
         struct vpfe_device *vpfe_dev = video_drvdata(file);
+        struct v4l2_rect rect = crop->c;
         int ret = 0;
 
         v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_crop\n");
@@ -1684,7 +1685,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
         if (ret)
                 return ret;
 
-        if (crop->c.top < 0 || crop->c.left < 0) {
+        if (rect.top < 0 || rect.left < 0) {
                 v4l2_err(&vpfe_dev->v4l2_dev,
                         "doesn't support negative values for top & left\n");
                 ret = -EINVAL;
@@ -1692,26 +1693,26 @@ static int vpfe_s_crop(struct file *file, void *priv,
         }
 
         /* adjust the width to 16 pixel boundary */
-        crop->c.width = ((crop->c.width + 15) & ~0xf);
+        rect.width = ((rect.width + 15) & ~0xf);
 
         /* make sure parameters are valid */
-        if ((crop->c.left + crop->c.width >
+        if ((rect.left + rect.width >
                 vpfe_dev->std_info.active_pixels) ||
-            (crop->c.top + crop->c.height >
+            (rect.top + rect.height >
                 vpfe_dev->std_info.active_lines)) {
                 v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_CROP params\n");
                 ret = -EINVAL;
                 goto unlock_out;
         }
-        ccdc_dev->hw_ops.set_image_window(&crop->c);
-        vpfe_dev->fmt.fmt.pix.width = crop->c.width;
-        vpfe_dev->fmt.fmt.pix.height = crop->c.height;
+        ccdc_dev->hw_ops.set_image_window(&rect);
+        vpfe_dev->fmt.fmt.pix.width = rect.width;
+        vpfe_dev->fmt.fmt.pix.height = rect.height;
         vpfe_dev->fmt.fmt.pix.bytesperline =
                 ccdc_dev->hw_ops.get_line_length();
         vpfe_dev->fmt.fmt.pix.sizeimage =
                 vpfe_dev->fmt.fmt.pix.bytesperline *
                 vpfe_dev->fmt.fmt.pix.height;
-        vpfe_dev->crop = crop->c;
+        vpfe_dev->crop = rect;
 unlock_out:
         mutex_unlock(&vpfe_dev->lock);
         return ret;
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 0bafecac4923..fcabc023885d 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -311,12 +311,13 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
         }
 
         /* configure 1 or 2 channel mode */
-        ret = vpif_config_data->setup_input_channel_mode
-                                        (vpif->std_info.ycmux_mode);
-
-        if (ret < 0) {
-                vpif_dbg(1, debug, "can't set vpif channel mode\n");
-                return ret;
+        if (vpif_config_data->setup_input_channel_mode) {
+                ret = vpif_config_data->
+                        setup_input_channel_mode(vpif->std_info.ycmux_mode);
+                if (ret < 0) {
+                        vpif_dbg(1, debug, "can't set vpif channel mode\n");
+                        return ret;
+                }
         }
 
         /* Call vpif_set_params function to set the parameters and addresses */
@@ -863,13 +864,11 @@ static unsigned int vpif_poll(struct file *filep, poll_table * wait)
  */
 static int vpif_open(struct file *filep)
 {
-        struct vpif_capture_config *config = vpif_dev->platform_data;
         struct video_device *vdev = video_devdata(filep);
         struct common_obj *common;
         struct video_obj *vid_ch;
         struct channel_obj *ch;
         struct vpif_fh *fh;
-        int i;
 
         vpif_dbg(2, debug, "vpif_open\n");
 
@@ -878,26 +877,6 @@ static int vpif_open(struct file *filep)
         vid_ch = &ch->video;
         common = &ch->common[VPIF_VIDEO_INDEX];
 
-        if (NULL == ch->curr_subdev_info) {
-                /**
-                 * search through the sub device to see a registered
-                 * sub device and make it as current sub device
-                 */
-                for (i = 0; i < config->subdev_count; i++) {
-                        if (vpif_obj.sd[i]) {
-                                /* the sub device is registered */
-                                ch->curr_subdev_info = &config->subdev_info[i];
-                                /* make first input as the current input */
-                                vid_ch->input_idx = 0;
-                                break;
-                        }
-                }
-                if (i == config->subdev_count) {
-                        vpif_err("No sub device registered\n");
-                        return -ENOENT;
-                }
-        }
-
         /* Allocate memory for the file handle object */
         fh = kzalloc(sizeof(struct vpif_fh), GFP_KERNEL);
         if (NULL == fh) {
@@ -997,6 +976,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
         struct common_obj *common;
         u8 index = 0;
         struct vb2_queue *q;
+        int ret;
 
         vpif_dbg(2, debug, "vpif_reqbufs\n");
 
@@ -1036,8 +1016,12 @@ static int vpif_reqbufs(struct file *file, void *priv,
         q->mem_ops = &vb2_dma_contig_memops;
         q->buf_struct_size = sizeof(struct vpif_cap_buffer);
 
-        vb2_queue_init(q);
-
+        ret = vb2_queue_init(q);
+        if (ret) {
+                vpif_err("vpif_capture: vb2_queue_init() failed\n");
+                vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+                return ret;
+        }
         /* Set io allowed member of file handle to TRUE */
         fh->io_allowed[index] = 1;
         /* Increment io usrs member of channel object to 1 */
@@ -1175,10 +1159,9 @@ static int vpif_streamon(struct file *file, void *priv,
                 return ret;
 
         /* Enable streamon on the sub device */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-                                s_stream, 1);
+        ret = v4l2_subdev_call(ch->sd, video, s_stream, 1);
 
-        if (ret && (ret != -ENOIOCTLCMD)) {
+        if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
                 vpif_dbg(1, debug, "stream on failed in subdev\n");
                 return ret;
         }
@@ -1238,73 +1221,105 @@ static int vpif_streamoff(struct file *file, void *priv,
 
         common->started = 0;
 
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-                                s_stream, 0);
+        ret = v4l2_subdev_call(ch->sd, video, s_stream, 0);
 
-        if (ret && (ret != -ENOIOCTLCMD))
+        if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
                 vpif_dbg(1, debug, "stream off failed in subdev\n");
 
         return vb2_streamoff(&common->buffer_queue, buftype);
 }
 
 /**
- * vpif_map_sub_device_to_input() - Maps sub device to input
- * @ch - ptr to channel
- * @config - ptr to capture configuration
+ * vpif_input_to_subdev() - Maps input to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
  * @input_index - Given input index from application
- * @sub_device_index - index into sd table
  *
  * lookup the sub device information for a given input index.
  * we report all the inputs to application. inputs table also
  * has sub device name for the each input
  */
-static struct vpif_subdev_info *vpif_map_sub_device_to_input(
-                                struct channel_obj *ch,
-                                struct vpif_capture_config *vpif_cfg,
-                                int input_index,
-                                int *sub_device_index)
+static int vpif_input_to_subdev(
+                struct vpif_capture_config *vpif_cfg,
+                struct vpif_capture_chan_config *chan_cfg,
+                int input_index)
 {
-        struct vpif_capture_chan_config *chan_cfg;
-        struct vpif_subdev_info *subdev_info = NULL;
-        const char *subdev_name = NULL;
+        struct vpif_subdev_info *subdev_info;
+        const char *subdev_name;
         int i;
 
-        vpif_dbg(2, debug, "vpif_map_sub_device_to_input\n");
+        vpif_dbg(2, debug, "vpif_input_to_subdev\n");
 
-        chan_cfg = &vpif_cfg->chan_config[ch->channel_id];
-
-        /**
-         * search through the inputs to find the sub device supporting
-         * the input
-         */
-        for (i = 0; i < chan_cfg->input_count; i++) {
-                /* For each sub device, loop through input */
-                if (i == input_index) {
-                        subdev_name = chan_cfg->inputs[i].subdev_name;
-                        break;
-                }
-        }
-
-        /* if reached maximum. return null */
-        if (i == chan_cfg->input_count || (NULL == subdev_name))
-                return subdev_info;
+        subdev_name = chan_cfg->inputs[input_index].subdev_name;
+        if (subdev_name == NULL)
+                return -1;
 
         /* loop through the sub device list to get the sub device info */
         for (i = 0; i < vpif_cfg->subdev_count; i++) {
                 subdev_info = &vpif_cfg->subdev_info[i];
                 if (!strcmp(subdev_info->name, subdev_name))
-                        break;
+                        return i;
+        }
+        return -1;
+}
+
+/**
+ * vpif_set_input() - Select an input
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @_index - Given input index from application
+ *
+ * Select the given input.
+ */
+static int vpif_set_input(
+                struct vpif_capture_config *vpif_cfg,
+                struct channel_obj *ch,
+                int index)
+{
+        struct vpif_capture_chan_config *chan_cfg =
+                        &vpif_cfg->chan_config[ch->channel_id];
+        struct vpif_subdev_info *subdev_info = NULL;
+        struct v4l2_subdev *sd = NULL;
+        u32 input = 0, output = 0;
+        int sd_index;
+        int ret;
+
+        sd_index = vpif_input_to_subdev(vpif_cfg, chan_cfg, index);
+        if (sd_index >= 0) {
+                sd = vpif_obj.sd[sd_index];
+                subdev_info = &vpif_cfg->subdev_info[sd_index];
         }
 
-        if (i == vpif_cfg->subdev_count)
-                return subdev_info;
+        /* first setup input path from sub device to vpif */
+        if (sd && vpif_cfg->setup_input_path) {
+                ret = vpif_cfg->setup_input_path(ch->channel_id,
+                                       subdev_info->name);
+                if (ret < 0) {
+                        vpif_dbg(1, debug, "couldn't setup input path for the" \
+                        " sub device %s, for input index %d\n",
+                        subdev_info->name, index);
+                        return ret;
+                }
+        }
 
-        /* check if the sub device is registered */
-        if (NULL == vpif_obj.sd[i])
-                return NULL;
+        if (sd) {
+                input = chan_cfg->inputs[index].input_route;
+                output = chan_cfg->inputs[index].output_route;
+                ret = v4l2_subdev_call(sd, video, s_routing,
+                                input, output, 0);
+                if (ret < 0 && ret != -ENOIOCTLCMD) {
+                        vpif_dbg(1, debug, "Failed to set input\n");
+                        return ret;
+                }
+        }
+        ch->input_idx = index;
+        ch->sd = sd;
+        /* copy interface parameters to vpif */
+        ch->vpifparams.iface = chan_cfg->vpif_if;
 
-        *sub_device_index = i;
-        return subdev_info;
+        /* update tvnorms from the sub device input info */
+        ch->video_dev->tvnorms = chan_cfg->inputs[index].input.std;
+        return 0;
 }
 
 /**
@@ -1324,12 +1339,16 @@ static int vpif_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
         vpif_dbg(2, debug, "vpif_querystd\n");
 
         /* Call querystd function of decoder device */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-                                querystd, std_id);
-        if (ret < 0)
-                vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
+        ret = v4l2_subdev_call(ch->sd, video, querystd, std_id);
 
-        return ret;
+        if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+                return -ENODATA;
+        if (ret) {
+                vpif_dbg(1, debug, "Failed to query standard for sub devices\n");
+                return ret;
+        }
+
+        return 0;
 }
 
 /**
@@ -1397,11 +1416,12 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
         vpif_config_format(ch);
 
         /* set standard in the sub device */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-                                s_std, *std_id);
-        if (ret < 0)
+        ret = v4l2_subdev_call(ch->sd, core, s_std, *std_id);
+        if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
                 vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
-        return ret;
+                return ret;
+        }
+        return 0;
 }
 
 /**
@@ -1441,10 +1461,8 @@ static int vpif_g_input(struct file *file, void *priv, unsigned int *index)
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
-
-        *index = vid_ch->input_idx;
 
+        *index = ch->input_idx;
         return 0;
 }
 
@@ -1461,13 +1479,13 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
         struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-        struct video_obj *vid_ch = &ch->video;
-        struct vpif_subdev_info *subdev_info;
-        int ret = 0, sd_index = 0;
-        u32 input = 0, output = 0;
+        int ret;
 
         chan_cfg = &config->chan_config[ch->channel_id];
 
+        if (index >= chan_cfg->input_count)
+                return -EINVAL;
+
         if (common->started) {
                 vpif_err("Streaming in progress\n");
                 return -EBUSY;
@@ -1486,45 +1504,7 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
                 return ret;
 
         fh->initialized = 1;
-        subdev_info = vpif_map_sub_device_to_input(ch, config, index,
-                                                   &sd_index);
-        if (NULL == subdev_info) {
-                vpif_dbg(1, debug,
-                        "couldn't lookup sub device for the input index\n");
-                return -EINVAL;
-        }
-
-        /* first setup input path from sub device to vpif */
-        if (config->setup_input_path) {
-                ret = config->setup_input_path(ch->channel_id,
-                                               subdev_info->name);
-                if (ret < 0) {
-                        vpif_dbg(1, debug, "couldn't setup input path for the"
-                                " sub device %s, for input index %d\n",
-                                subdev_info->name, index);
-                        return ret;
-                }
-        }
-
-        if (subdev_info->can_route) {
-                input = subdev_info->input;
-                output = subdev_info->output;
-                ret = v4l2_subdev_call(vpif_obj.sd[sd_index], video, s_routing,
-                                        input, output, 0);
-                if (ret < 0) {
-                        vpif_dbg(1, debug, "Failed to set input\n");
-                        return ret;
-                }
-        }
-        vid_ch->input_idx = index;
-        ch->curr_subdev_info = subdev_info;
-        ch->curr_sd_index = sd_index;
-        /* copy interface parameters to vpif */
-        ch->vpifparams.iface = subdev_info->vpif_if;
-
-        /* update tvnorms from the sub device input info */
-        ch->video_dev->tvnorms = chan_cfg->inputs[index].input.std;
-        return ret;
+        return vpif_set_input(config, ch, index);
 }
 
 /**
@@ -1655,9 +1635,11 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
         struct vpif_capture_config *config = vpif_dev->platform_data;
 
-        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-        strlcpy(cap->driver, "vpif capture", sizeof(cap->driver));
-        strlcpy(cap->bus_info, "VPIF Platform", sizeof(cap->bus_info));
+        cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+        snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+                 dev_name(vpif_dev));
         strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
         return 0;
@@ -1730,9 +1712,12 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
+        int ret;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-                                video, enum_dv_timings, timings);
+        ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+                return -EINVAL;
+        return ret;
 }
 
 /**
@@ -1747,9 +1732,12 @@ vpif_query_dv_timings(struct file *file, void *priv,
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
+        int ret;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-                                video, query_dv_timings, timings);
+        ret = v4l2_subdev_call(ch->sd, video, query_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+                return -ENODATA;
+        return ret;
 }
 
 /**
@@ -1775,13 +1763,9 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
         }
 
         /* Configure subdevice timings, if any */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-                        video, s_dv_timings, timings);
-        if (ret == -ENOIOCTLCMD) {
-                vpif_dbg(2, debug, "Custom DV timings not supported by "
-                                "subdevice\n");
-                return -EINVAL;
-        }
+        ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+                ret = 0;
         if (ret < 0) {
                 vpif_dbg(2, debug, "Error setting custom DV timings\n");
                 return ret;
@@ -1906,8 +1890,7 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-                        g_register, reg);
+        return v4l2_subdev_call(ch->sd, core, g_register, reg);
 }
 
 /*
@@ -1924,8 +1907,7 @@ static int vpif_dbg_s_register(struct file *file, void *priv,
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-                        s_register, reg);
+        return v4l2_subdev_call(ch->sd, core, s_register, reg);
 }
 #endif
 
@@ -2063,7 +2045,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 {
         struct vpif_subdev_info *subdevdata;
         struct vpif_capture_config *config;
-        int i, j, k, m, q, err;
+        int i, j, k, err;
+        int res_idx = 0;
         struct i2c_adapter *i2c_adap;
         struct channel_obj *ch;
         struct common_obj *common;
@@ -2086,18 +2069,19 @@ static __init int vpif_probe(struct platform_device *pdev)
                 return err;
         }
 
-        k = 0;
-        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
+        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
                 for (i = res->start; i <= res->end; i++) {
                         if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
-                                        "VPIF_Capture",
-                                (void *)(&vpif_obj.dev[k]->channel_id))) {
+                                        "VPIF_Capture", (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id))) {
                                 err = -EBUSY;
-                                i--;
+                                for (j = 0; j < i; j++)
+                                        free_irq(j, (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id));
                                 goto vpif_int_err;
                         }
                 }
-                k++;
+                res_idx++;
         }
 
         for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
@@ -2111,7 +2095,7 @@ static __init int vpif_probe(struct platform_device *pdev)
                                 video_device_release(ch->video_dev);
                         }
                         err = -ENOMEM;
-                        goto vpif_dev_alloc_err;
+                        goto vpif_int_err;
                 }
 
                 /* Initialize field of video device */
@@ -2142,24 +2126,6 @@ static __init int vpif_probe(struct platform_device *pdev)
                 }
         }
 
-        for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
-                ch = vpif_obj.dev[j];
-                ch->channel_id = j;
-                common = &(ch->common[VPIF_VIDEO_INDEX]);
-                spin_lock_init(&common->irqlock);
-                mutex_init(&common->lock);
-                ch->video_dev->lock = &common->lock;
-                /* Initialize prio member of channel object */
-                v4l2_prio_init(&ch->prio);
-                err = video_register_device(ch->video_dev,
-                                            VFL_TYPE_GRABBER, (j ? 1 : 0));
-                if (err)
-                        goto probe_out;
-
-                video_set_drvdata(ch->video_dev, ch);
-
-        }
-
         i2c_adap = i2c_get_adapter(1);
         config = pdev->dev.platform_data;
 
@@ -2169,7 +2135,7 @@ static __init int vpif_probe(struct platform_device *pdev)
         if (vpif_obj.sd == NULL) {
                 vpif_err("unable to allocate memory for subdevice pointers\n");
                 err = -ENOMEM;
-                goto probe_out;
+                goto vpif_sd_error;
         }
 
         for (i = 0; i < subdev_count; i++) {
@@ -2186,19 +2152,32 @@ static __init int vpif_probe(struct platform_device *pdev)
                 }
                 v4l2_info(&vpif_obj.v4l2_dev, "registered sub device %s\n",
                           subdevdata->name);
-
-                if (vpif_obj.sd[i])
-                        vpif_obj.sd[i]->grp_id = 1 << i;
         }
 
+        for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
+                ch = vpif_obj.dev[j];
+                ch->channel_id = j;
+                common = &(ch->common[VPIF_VIDEO_INDEX]);
+                spin_lock_init(&common->irqlock);
+                mutex_init(&common->lock);
+                ch->video_dev->lock = &common->lock;
+                /* Initialize prio member of channel object */
+                v4l2_prio_init(&ch->prio);
+                video_set_drvdata(ch->video_dev, ch);
+
+                /* select input 0 */
+                err = vpif_set_input(config, ch, 0);
+                if (err)
+                        goto probe_out;
+
+                err = video_register_device(ch->video_dev,
+                                            VFL_TYPE_GRABBER, (j ? 1 : 0));
+                if (err)
+                        goto probe_out;
+        }
         v4l2_info(&vpif_obj.v4l2_dev, "VPIF capture driver initialized\n");
         return 0;
 
-probe_subdev_out:
-        /* free sub devices memory */
-        kfree(vpif_obj.sd);
-
-        j = VPIF_CAPTURE_MAX_DEVICES;
 probe_out:
         for (k = 0; k < j; k++) {
                 /* Get the pointer to the channel object */
@@ -2206,22 +2185,23 @@ probe_out:
                 /* Unregister video device */
                 video_unregister_device(ch->video_dev);
         }
+probe_subdev_out:
+        /* free sub devices memory */
+        kfree(vpif_obj.sd);
 
-vpif_dev_alloc_err:
-        k = VPIF_CAPTURE_MAX_DEVICES-1;
-        res = platform_get_resource(pdev, IORESOURCE_IRQ, k);
-        i = res->end;
-
-vpif_int_err:
-        for (q = k; q >= 0; q--) {
-                for (m = i; m >= (int)res->start; m--)
-                        free_irq(m, (void *)(&vpif_obj.dev[q]->channel_id));
-
-                res = platform_get_resource(pdev, IORESOURCE_IRQ, q-1);
-                if (res)
-                        i = res->end;
+vpif_sd_error:
+        for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+                ch = vpif_obj.dev[i];
+                /* Note: does nothing if ch->video_dev == NULL */
+                video_device_release(ch->video_dev);
         }
+vpif_int_err:
         v4l2_device_unregister(&vpif_obj.v4l2_dev);
+        for (i = 0; i < res_idx; i++) {
+                res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+                for (j = res->start; j <= res->end; j++)
+                        free_irq(j, (void *)(&vpif_obj.dev[i]->channel_id));
+        }
         return err;
 }
 
diff --git a/drivers/media/platform/davinci/vpif_capture.h b/drivers/media/platform/davinci/vpif_capture.h
index d24efc17e4c8..3d3c1e5cd5d4 100644
--- a/drivers/media/platform/davinci/vpif_capture.h
+++ b/drivers/media/platform/davinci/vpif_capture.h
@@ -54,8 +54,6 @@ struct video_obj {
         /* Currently selected or default standard */
         v4l2_std_id stdid;
         struct v4l2_dv_timings dv_timings;
-        /* This is to track the last input that is passed to application */
-        u32 input_idx;
 };
 
 struct vpif_cap_buffer {
@@ -119,10 +117,10 @@ struct channel_obj {
         u8 initialized;
         /* Identifies channel */
         enum vpif_channel_id channel_id;
-        /* index into sd table */
-        int curr_sd_index;
-        /* ptr to current sub device information */
-        struct vpif_subdev_info *curr_subdev_info;
+        /* Current input */
+        u32 input_idx;
+        /* subdev corresponding to the current input, may be NULL */
+        struct v4l2_subdev *sd;
         /* vpif configuration params */
         struct vpif_params vpifparams;
         /* common object array */
@@ -159,10 +157,6 @@ struct vpif_config_params {
         u32 video_limit[VPIF_CAPTURE_NUM_CHANNELS];
         u8 max_device_type;
 };
-/* Struct which keeps track of the line numbers for the sliced vbi service */
-struct vpif_service_line {
-        u16 service_id;
-        u16 service_line[2];
-};
+
 #endif                          /* End of __KERNEL__ */
 #endif                          /* VPIF_CAPTURE_H */
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index a5b88689abad..b716fbd4241f 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -280,12 +280,13 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
         }
 
         /* clock settings */
-        ret =
-            vpif_config_data->set_clock(ch->vpifparams.std_info.ycmux_mode,
-                                        ch->vpifparams.std_info.hd_sd);
-        if (ret < 0) {
-                vpif_err("can't set clock\n");
-                return ret;
+        if (vpif_config_data->set_clock) {
+                ret = vpif_config_data->set_clock(ch->vpifparams.std_info.
+                ycmux_mode, ch->vpifparams.std_info.hd_sd);
+                if (ret < 0) {
+                        vpif_err("can't set clock\n");
+                        return ret;
+                }
         }
 
         /* set the parameters and addresses */
@@ -307,7 +308,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
                 channel2_intr_assert();
                 channel2_intr_enable(1);
                 enable_channel2(1);
-                if (vpif_config_data->ch2_clip_en)
+                if (vpif_config_data->chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
                         channel2_clipping_enable(1);
         }
 
@@ -316,7 +317,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
                 channel3_intr_assert();
                 channel3_intr_enable(1);
                 enable_channel3(1);
-                if (vpif_config_data->ch3_clip_en)
+                if (vpif_config_data->chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
                         channel3_clipping_enable(1);
         }
 
@@ -826,9 +827,11 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
         struct vpif_display_config *config = vpif_dev->platform_data;
 
-        cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
-        strlcpy(cap->driver, "vpif display", sizeof(cap->driver));
-        strlcpy(cap->bus_info, "Platform", sizeof(cap->bus_info));
+        cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+        snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+                 dev_name(vpif_dev));
         strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
         return 0;
@@ -935,6 +938,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
         enum v4l2_field field;
         struct vb2_queue *q;
         u8 index = 0;
+        int ret;
 
         /* This file handle has not initialized the channel,
            It is not allowed to do settings */
@@ -980,8 +984,12 @@ static int vpif_reqbufs(struct file *file, void *priv,
         q->mem_ops = &vb2_dma_contig_memops;
         q->buf_struct_size = sizeof(struct vpif_disp_buffer);
 
-        vb2_queue_init(q);
-
+        ret = vb2_queue_init(q);
+        if (ret) {
+                vpif_err("vpif_display: vb2_queue_init() failed\n");
+                vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+                return ret;
+        }
         /* Set io allowed member of file handle to TRUE */
         fh->io_allowed[index] = 1;
         /* Increment io usrs member of channel object to 1 */
@@ -1173,14 +1181,16 @@ static int vpif_streamoff(struct file *file, void *priv,
         if (buftype == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
                 /* disable channel */
                 if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-                        if (vpif_config_data->ch2_clip_en)
+                        if (vpif_config_data->
+                                chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
                                 channel2_clipping_enable(0);
                         enable_channel2(0);
                         channel2_intr_enable(0);
                 }
                 if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
                                         (2 == common->started)) {
-                        if (vpif_config_data->ch3_clip_en)
+                        if (vpif_config_data->
+                                chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
                                 channel3_clipping_enable(0);
                         enable_channel3(0);
                         channel3_intr_enable(0);
@@ -1213,49 +1223,126 @@ static int vpif_enum_output(struct file *file, void *fh,
 {
 
         struct vpif_display_config *config = vpif_dev->platform_data;
+        struct vpif_display_chan_config *chan_cfg;
+        struct vpif_fh *vpif_handler = fh;
+        struct channel_obj *ch = vpif_handler->channel;
 
-        if (output->index >= config->output_count) {
+        chan_cfg = &config->chan_config[ch->channel_id];
+        if (output->index >= chan_cfg->output_count) {
                 vpif_dbg(1, debug, "Invalid output index\n");
                 return -EINVAL;
         }
 
-        strcpy(output->name, config->output[output->index]);
-        output->type = V4L2_OUTPUT_TYPE_ANALOG;
-        output->std = VPIF_V4L2_STD;
+        *output = chan_cfg->outputs[output->index].output;
+        return 0;
+}
+
+/**
+ * vpif_output_to_subdev() - Maps output to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
+ * @index - Given output index from application
+ *
+ * lookup the sub device information for a given output index.
+ * we report all the output to application. output table also
+ * has sub device name for the each output
+ */
+static int
+vpif_output_to_subdev(struct vpif_display_config *vpif_cfg,
+                      struct vpif_display_chan_config *chan_cfg, int index)
+{
+        struct vpif_subdev_info *subdev_info;
+        const char *subdev_name;
+        int i;
+
+        vpif_dbg(2, debug, "vpif_output_to_subdev\n");
+
+        if (chan_cfg->outputs == NULL)
+                return -1;
+
+        subdev_name = chan_cfg->outputs[index].subdev_name;
+        if (subdev_name == NULL)
+                return -1;
+
+        /* loop through the sub device list to get the sub device info */
+        for (i = 0; i < vpif_cfg->subdev_count; i++) {
+                subdev_info = &vpif_cfg->subdevinfo[i];
+                if (!strcmp(subdev_info->name, subdev_name))
+                        return i;
+        }
+        return -1;
+}
+
+/**
+ * vpif_set_output() - Select an output
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @index - Given output index from application
+ *
+ * Select the given output.
+ */
+static int vpif_set_output(struct vpif_display_config *vpif_cfg,
+                      struct channel_obj *ch, int index)
+{
+        struct vpif_display_chan_config *chan_cfg =
+                &vpif_cfg->chan_config[ch->channel_id];
+        struct vpif_subdev_info *subdev_info = NULL;
+        struct v4l2_subdev *sd = NULL;
+        u32 input = 0, output = 0;
+        int sd_index;
+        int ret;
+
+        sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index);
+        if (sd_index >= 0) {
+                sd = vpif_obj.sd[sd_index];
+                subdev_info = &vpif_cfg->subdevinfo[sd_index];
+        }
+
+        if (sd) {
+                input = chan_cfg->outputs[index].input_route;
+                output = chan_cfg->outputs[index].output_route;
+                ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
+                if (ret < 0 && ret != -ENOIOCTLCMD) {
+                        vpif_err("Failed to set output\n");
+                        return ret;
+                }
 
+        }
+        ch->output_idx = index;
+        ch->sd = sd;
+        if (chan_cfg->outputs != NULL)
+                /* update tvnorms from the sub device output info */
+                ch->video_dev->tvnorms = chan_cfg->outputs[index].output.std;
         return 0;
 }
 
 static int vpif_s_output(struct file *file, void *priv, unsigned int i)
 {
+        struct vpif_display_config *config = vpif_dev->platform_data;
+        struct vpif_display_chan_config *chan_cfg;
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
         struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-        int ret = 0;
+
+        chan_cfg = &config->chan_config[ch->channel_id];
+
+        if (i >= chan_cfg->output_count)
+                return -EINVAL;
 
         if (common->started) {
                 vpif_err("Streaming in progress\n");
                 return -EBUSY;
         }
 
-        ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
-                                                        s_routing, 0, i, 0);
-
-        if (ret < 0)
-                vpif_err("Failed to set output standard\n");
-
-        vid_ch->output_id = i;
-        return ret;
+        return vpif_set_output(config, ch, i);
 }
 
 static int vpif_g_output(struct file *file, void *priv, unsigned int *i)
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
 
-        *i = vid_ch->output_id;
+        *i = ch->output_idx;
 
         return 0;
 }
@@ -1290,10 +1377,12 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
+        int ret;
 
-        return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id],
-                        video, enum_dv_timings, timings);
+        ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+                return -EINVAL;
+        return ret;
 }
 
 /**
@@ -1319,13 +1408,9 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
         }
 
         /* Configure subdevice timings, if any */
-        ret = v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id],
-                        video, s_dv_timings, timings);
-        if (ret == -ENOIOCTLCMD) {
-                vpif_dbg(2, debug, "Custom DV timings not supported by "
-                                "subdevice\n");
-                return -EINVAL;
-        }
+        ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+                ret = 0;
         if (ret < 0) {
                 vpif_dbg(2, debug, "Error setting custom DV timings\n");
                 return ret;
@@ -1450,10 +1535,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
                 struct v4l2_dbg_register *reg){
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
 
-        return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id], core,
-                        g_register, reg);
+        return v4l2_subdev_call(ch->sd, core, g_register, reg);
 }
 
 /*
@@ -1469,10 +1552,8 @@ static int vpif_dbg_s_register(struct file *file, void *priv,
                 struct v4l2_dbg_register *reg){
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
 
-        return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id], core,
-                        s_register, reg);
+        return v4l2_subdev_call(ch->sd, core, s_register, reg);
 }
 #endif
 
@@ -1536,9 +1617,6 @@ static struct video_device vpif_video_template = {
         .name           = "vpif",
         .fops           = &vpif_fops,
         .ioctl_ops      = &vpif_ioctl_ops,
-        .tvnorms        = VPIF_V4L2_STD,
-        .current_norm   = V4L2_STD_625_50,
-
 };
 
 /*Configure the channels, buffer sizei, request irq */
@@ -1611,7 +1689,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 {
         struct vpif_subdev_info *subdevdata;
         struct vpif_display_config *config;
-        int i, j = 0, k, q, m, err = 0;
+        int i, j = 0, k, err = 0;
+        int res_idx = 0;
         struct i2c_adapter *i2c_adap;
         struct common_obj *common;
         struct channel_obj *ch;
@@ -1634,21 +1713,22 @@ static __init int vpif_probe(struct platform_device *pdev)
                 return err;
         }
 
-        k = 0;
-        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
+        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
                 for (i = res->start; i <= res->end; i++) {
                         if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
-                                        "VPIF_Display",
-                                (void *)(&vpif_obj.dev[k]->channel_id))) {
+                                        "VPIF_Display", (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id))) {
                                 err = -EBUSY;
+                                for (j = 0; j < i; j++)
+                                        free_irq(j, (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id));
                                 goto vpif_int_err;
                         }
                 }
-                k++;
+                res_idx++;
         }
 
         for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
-
                 /* Get the pointer to the channel object */
                 ch = vpif_obj.dev[i];
 
@@ -1694,6 +1774,32 @@ static __init int vpif_probe(struct platform_device *pdev)
                 }
         }
 
+        i2c_adap = i2c_get_adapter(1);
+        config = pdev->dev.platform_data;
+        subdev_count = config->subdev_count;
+        subdevdata = config->subdevinfo;
+        vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
+                                                                GFP_KERNEL);
+        if (vpif_obj.sd == NULL) {
+                vpif_err("unable to allocate memory for subdevice pointers\n");
+                err = -ENOMEM;
+                goto vpif_sd_error;
+        }
+
+        for (i = 0; i < subdev_count; i++) {
+                vpif_obj.sd[i] = v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
+                                                i2c_adap,
+                                                &subdevdata[i].board_info,
+                                                NULL);
+                if (!vpif_obj.sd[i]) {
+                        vpif_err("Error registering v4l2 subdevice\n");
+                        goto probe_subdev_out;
+                }
+
+                if (vpif_obj.sd[i])
+                        vpif_obj.sd[i]->grp_id = 1 << i;
+        }
+
         for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
                 ch = vpif_obj.dev[j];
                 /* Initialize field of the channel objects */
@@ -1715,6 +1821,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 
                 }
                 ch->initialized = 0;
+                if (subdev_count)
+                        ch->sd = vpif_obj.sd[0];
                 ch->channel_id = j;
                 if (j < 2)
                         ch->common[VPIF_VIDEO_INDEX].numbuffers =
@@ -1729,6 +1837,12 @@ static __init int vpif_probe(struct platform_device *pdev)
                 ch->common[VPIF_VIDEO_INDEX].fmt.type =
                                                 V4L2_BUF_TYPE_VIDEO_OUTPUT;
                 ch->video_dev->lock = &common->lock;
+                video_set_drvdata(ch->video_dev, ch);
+
+                /* select output 0 */
+                err = vpif_set_output(config, ch, 0);
+                if (err)
+                        goto probe_out;
 
                 /* register video device */
                 vpif_dbg(1, debug, "channel=%x,channel->video_dev=%x\n",
@@ -1738,42 +1852,12 @@ static __init int vpif_probe(struct platform_device *pdev)
                                           VFL_TYPE_GRABBER, (j ? 3 : 2));
                 if (err < 0)
                         goto probe_out;
-
-                video_set_drvdata(ch->video_dev, ch);
-        }
-
-        i2c_adap = i2c_get_adapter(1);
-        config = pdev->dev.platform_data;
-        subdev_count = config->subdev_count;
-        subdevdata = config->subdevinfo;
-        vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
-                                                                GFP_KERNEL);
-        if (vpif_obj.sd == NULL) {
-                vpif_err("unable to allocate memory for subdevice pointers\n");
-                err = -ENOMEM;
-                goto probe_out;
-        }
-
-        for (i = 0; i < subdev_count; i++) {
-                vpif_obj.sd[i] = v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
-                                                i2c_adap,
-                                                &subdevdata[i].board_info,
-                                                NULL);
-                if (!vpif_obj.sd[i]) {
-                        vpif_err("Error registering v4l2 subdevice\n");
-                        goto probe_subdev_out;
-                }
-
-                if (vpif_obj.sd[i])
-                        vpif_obj.sd[i]->grp_id = 1 << i;
         }
 
         v4l2_info(&vpif_obj.v4l2_dev,
                         " VPIF display driver initialized\n");
         return 0;
 
-probe_subdev_out:
-        kfree(vpif_obj.sd);
 probe_out:
         for (k = 0; k < j; k++) {
                 ch = vpif_obj.dev[k];
@@ -1781,14 +1865,21 @@ probe_out:
                 video_device_release(ch->video_dev);
                 ch->video_dev = NULL;
         }
+probe_subdev_out:
+        kfree(vpif_obj.sd);
+vpif_sd_error:
+        for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+                ch = vpif_obj.dev[i];
+                /* Note: does nothing if ch->video_dev == NULL */
+                video_device_release(ch->video_dev);
+        }
 vpif_int_err:
         v4l2_device_unregister(&vpif_obj.v4l2_dev);
         vpif_err("VPIF IRQ request failed\n");
-        for (q = k; k >= 0; k--) {
-                for (m = i; m >= res->start; m--)
-                        free_irq(m, (void *)(&vpif_obj.dev[k]->channel_id));
-                res = platform_get_resource(pdev, IORESOURCE_IRQ, k-1);
-                m = res->end;
+        for (i = 0; i < res_idx; i++) {
+                res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+                for (j = res->start; j <= res->end; j++)
+                        free_irq(j, (void *)(&vpif_obj.dev[i]->channel_id));
         }
 
         return err;
diff --git a/drivers/media/platform/davinci/vpif_display.h b/drivers/media/platform/davinci/vpif_display.h
index f628ebcf3674..a5a18f74395c 100644
--- a/drivers/media/platform/davinci/vpif_display.h
+++ b/drivers/media/platform/davinci/vpif_display.h
@@ -62,13 +62,6 @@ struct video_obj {
         v4l2_std_id stdid;              /* Currently selected or default
                                          * standard */
         struct v4l2_dv_timings dv_timings;
-        u32 output_id;                  /* Current output id */
-};
-
-struct vbi_obj {
-        int num_services;
-        struct vpif_vbi_params vbiparams;       /* vpif parameters for the raw
-                                                 * vbi data */
 };
 
 struct vpif_disp_buffer {
@@ -131,12 +124,13 @@ struct channel_obj {
                                          * which is being displayed */
         u8 initialized;                 /* flag to indicate whether
                                          * encoder is initialized */
+        u32 output_idx;                 /* Current output index */
+        struct v4l2_subdev *sd;         /* Current output subdev(may be NULL) */
 
         enum vpif_channel_id channel_id;/* Identifies channel */
         struct vpif_params vpifparams;
         struct common_obj common[VPIF_NUMOBJECTS];
         struct video_obj video;
-        struct vbi_obj vbi;
 };
 
 /* File handle structure */
@@ -168,12 +162,4 @@ struct vpif_config_params {
         u8 min_numbuffers;
 };
 
-/* Struct which keeps track of the line numbers for the sliced vbi service */
-struct vpif_service_line {
-        u16 service_id;
-        u16 service_line[2];
-        u16 enc_service_id;
-        u8 bytestowrite;
-};
-
 #endif                          /* DAVINCIHD_DISPLAY_H */
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
index 0d8625f03a32..0146b354dc22 100644
--- a/drivers/media/platform/exynos-gsc/gsc-regs.c
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -212,7 +212,7 @@ void gsc_hw_set_in_image_format(struct gsc_ctx *ctx)
                 else
                         cfg |= GSC_IN_YUV422_3P;
                 break;
-        };
+        }
 
         writel(cfg, dev->regs + GSC_IN_CON);
 }
@@ -332,7 +332,7 @@ void gsc_hw_set_out_image_format(struct gsc_ctx *ctx)
         case 3:
                 cfg |= GSC_OUT_YUV420_3P;
                 break;
-        };
+        }
 
 end_set:
         writel(cfg, dev->regs + GSC_OUT_CON);
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 897250b88647..31ac4dc69247 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -864,7 +864,7 @@ int vidioc_s_fbuf(struct file *file, void *priv, const struct v4l2_framebuffer *
 {
         struct viu_fh  *fh = priv;
         struct viu_dev *dev = fh->dev;
-        struct v4l2_framebuffer *fb = arg;
+        const struct v4l2_framebuffer *fb = arg;
         struct viu_fmt *fmt;
 
         if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index d03637537118..2e2121e98133 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -397,8 +397,7 @@ static void device_isr(unsigned long priv)
         curr_ctx = v4l2_m2m_get_curr_priv(m2mtest_dev->m2m_dev);
 
         if (NULL == curr_ctx) {
-                printk(KERN_ERR
-                        "Instance released before the end of transaction\n");
+                pr_err("Instance released before the end of transaction\n");
                 return;
         }
 
@@ -894,7 +893,7 @@ static int m2mtest_open(struct file *file)
 
         if (mutex_lock_interruptible(&dev->dev_mutex))
                 return -ERESTARTSYS;
-        ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+        ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
         if (!ctx) {
                 rc = -ENOMEM;
                 goto open_unlock;
@@ -1020,7 +1019,7 @@ static int m2mtest_probe(struct platform_device *pdev)
         struct video_device *vfd;
         int ret;
 
-        dev = kzalloc(sizeof *dev, GFP_KERNEL);
+        dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
         if (!dev)
                 return -ENOMEM;
 
@@ -1028,7 +1027,7 @@ static int m2mtest_probe(struct platform_device *pdev)
 
         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
         if (ret)
-                goto free_dev;
+                return ret;
 
         atomic_set(&dev->num_inst, 0);
         mutex_init(&dev->dev_mutex);
@@ -1067,15 +1066,13 @@ static int m2mtest_probe(struct platform_device *pdev)
 
         return 0;
 
-        v4l2_m2m_release(dev->m2m_dev);
 err_m2m:
+        v4l2_m2m_release(dev->m2m_dev);
         video_unregister_device(dev->vfd);
 rel_vdev:
         video_device_release(vfd);
 unreg_dev:
         v4l2_device_unregister(&dev->v4l2_dev);
-free_dev:
-        kfree(dev);
 
         return ret;
 }
@@ -1090,7 +1087,6 @@ static int m2mtest_remove(struct platform_device *pdev)
         del_timer_sync(&dev->timer);
         video_unregister_device(dev->vfd);
         v4l2_device_unregister(&dev->v4l2_dev);
-        kfree(dev);
 
         return 0;
 }
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 134016f0e660..a3b1a34c896d 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -455,11 +455,15 @@ static int omapvid_init(struct omap_vout_device *vout, u32 addr)
 
         win = &vout->win;
         for (i = 0; i < ovid->num_overlays; i++) {
+                struct omap_dss_device *dssdev;
+
                 ovl = ovid->overlays[i];
-                if (!ovl->manager || !ovl->manager->device)
+                dssdev = ovl->get_device(ovl);
+
+                if (!dssdev)
                         return -EINVAL;
 
-                timing = &ovl->manager->device->panel.timings;
+                timing = &dssdev->panel.timings;
 
                 outw = win->w.width;
                 outh = win->w.height;
@@ -516,8 +520,11 @@ static int omapvid_apply_changes(struct omap_vout_device *vout)
         struct omapvideo_info *ovid = &vout->vid_info;
 
         for (i = 0; i < ovid->num_overlays; i++) {
+                struct omap_dss_device *dssdev;
+
                 ovl = ovid->overlays[i];
-                if (!ovl->manager || !ovl->manager->device)
+                dssdev = ovl->get_device(ovl);
+                if (!dssdev)
                         return -EINVAL;
                 ovl->manager->apply(ovl->manager);
         }
@@ -580,12 +587,14 @@ static void omap_vout_isr(void *arg, unsigned int irqstatus)
 
         ovid = &vout->vid_info;
         ovl = ovid->overlays[0];
-        /* get the display device attached to the overlay */
-        if (!ovl->manager || !ovl->manager->device)
-                return;
 
         mgr_id = ovl->manager->id;
-        cur_display = ovl->manager->device;
+
+        /* get the display device attached to the overlay */
+        cur_display = ovl->get_device(ovl);
+
+        if (!cur_display)
+                return;
 
         spin_lock(&vout->vbq_lock);
         do_gettimeofday(&timevalue);
@@ -949,7 +958,9 @@ static int omap_vout_release(struct file *file)
         /* Disable all the overlay managers connected with this interface */
         for (i = 0; i < ovid->num_overlays; i++) {
                 struct omap_overlay *ovl = ovid->overlays[i];
-                if (ovl->manager && ovl->manager->device)
+                struct omap_dss_device *dssdev = ovl->get_device(ovl);
+
+                if (dssdev)
                         ovl->disable(ovl);
         }
         /* Turn off the pipeline */
@@ -1082,14 +1093,17 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *fh,
         struct omapvideo_info *ovid;
         struct omap_video_timings *timing;
         struct omap_vout_device *vout = fh;
+        struct omap_dss_device *dssdev;
 
         ovid = &vout->vid_info;
         ovl = ovid->overlays[0];
+        /* get the display device attached to the overlay */
+        dssdev = ovl->get_device(ovl);
 
-        if (!ovl->manager || !ovl->manager->device)
+        if (!dssdev)
                 return -EINVAL;
-        /* get the display device attached to the overlay */
-        timing = &ovl->manager->device->panel.timings;
+
+        timing = &dssdev->panel.timings;
 
         vout->fbuf.fmt.height = timing->y_res;
         vout->fbuf.fmt.width = timing->x_res;
@@ -1106,6 +1120,7 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
         struct omapvideo_info *ovid;
         struct omap_video_timings *timing;
         struct omap_vout_device *vout = fh;
+        struct omap_dss_device *dssdev;
 
         if (vout->streaming)
                 return -EBUSY;
@@ -1114,13 +1129,14 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
 
         ovid = &vout->vid_info;
         ovl = ovid->overlays[0];
+        dssdev = ovl->get_device(ovl);
 
         /* get the display device attached to the overlay */
-        if (!ovl->manager || !ovl->manager->device) {
+        if (!dssdev) {
                 ret = -EINVAL;
                 goto s_fmt_vid_out_exit;
         }
-        timing = &ovl->manager->device->panel.timings;
+        timing = &dssdev->panel.timings;
 
         /* We dont support RGB24-packed mode if vrfb rotation
          * is enabled*/
@@ -1299,6 +1315,7 @@ static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr
         struct omapvideo_info *ovid;
         struct omap_overlay *ovl;
         struct omap_video_timings *timing;
+        struct omap_dss_device *dssdev;
 
         if (vout->streaming)
                 return -EBUSY;
@@ -1306,13 +1323,15 @@ static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr
         mutex_lock(&vout->lock);
         ovid = &vout->vid_info;
         ovl = ovid->overlays[0];
+        /* get the display device attached to the overlay */
+        dssdev = ovl->get_device(ovl);
 
-        if (!ovl->manager || !ovl->manager->device) {
+        if (!dssdev) {
                 ret = -EINVAL;
                 goto s_crop_err;
         }
-        /* get the display device attached to the overlay */
-        timing = &ovl->manager->device->panel.timings;
+
+        timing = &dssdev->panel.timings;
 
         if (is_rotation_90_or_270(vout)) {
                 vout->fbuf.fmt.height = timing->x_res;
@@ -1668,7 +1687,7 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
         for (j = 0; j < ovid->num_overlays; j++) {
                 struct omap_overlay *ovl = ovid->overlays[j];
 
-                if (ovl->manager && ovl->manager->device) {
+                if (ovl->get_device(ovl)) {
                         struct omap_overlay_info info;
                         ovl->get_overlay_info(ovl, &info);
                         info.paddr = addr;
@@ -1691,8 +1710,9 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 
         for (j = 0; j < ovid->num_overlays; j++) {
                 struct omap_overlay *ovl = ovid->overlays[j];
+                struct omap_dss_device *dssdev = ovl->get_device(ovl);
 
-                if (ovl->manager && ovl->manager->device) {
+                if (dssdev) {
                         ret = ovl->enable(ovl);
                         if (ret)
                                 goto streamon_err1;
@@ -1727,8 +1747,9 @@ static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 
         for (j = 0; j < ovid->num_overlays; j++) {
                 struct omap_overlay *ovl = ovid->overlays[j];
+                struct omap_dss_device *dssdev = ovl->get_device(ovl);
 
-                if (ovl->manager && ovl->manager->device)
+                if (dssdev)
                         ovl->disable(ovl);
         }
 
@@ -1891,8 +1912,8 @@ static int __init omap_vout_setup_video_data(struct omap_vout_device *vout)
         struct video_device *vfd;
         struct v4l2_pix_format *pix;
         struct v4l2_control *control;
-        struct omap_dss_device *display =
-                vout->vid_info.overlays[0]->manager->device;
+        struct omap_overlay *ovl = vout->vid_info.overlays[0];
+        struct omap_dss_device *display = ovl->get_device(ovl);
 
         /* set the default pix */
         pix = &vout->pix;
@@ -2207,8 +2228,10 @@ static int __init omap_vout_probe(struct platform_device *pdev)
          */
         for (i = 1; i < vid_dev->num_overlays; i++) {
                 ovl = omap_dss_get_overlay(i);
-                if (ovl->manager && ovl->manager->device) {
-                        def_display = ovl->manager->device;
+                dssdev = ovl->get_device(ovl);
+
+                if (dssdev) {
+                        def_display = dssdev;
                 } else {
                         dev_warn(&pdev->dev, "cannot find display\n");
                         def_display = NULL;
@@ -2255,8 +2278,10 @@ probe_err1:
         for (i = 1; i < vid_dev->num_overlays; i++) {
                 def_display = NULL;
                 ovl = omap_dss_get_overlay(i);
-                if (ovl->manager && ovl->manager->device)
-                        def_display = ovl->manager->device;
+                dssdev = ovl->get_device(ovl);
+
+                if (dssdev)
+                        def_display = dssdev;
 
                 if (def_display && def_display->driver)
                         def_display->driver->disable(def_display);
diff --git a/drivers/media/platform/omap3isp/ispreg.h b/drivers/media/platform/omap3isp/ispreg.h
index 084ea77d65a7..e2c57f334c5d 100644
--- a/drivers/media/platform/omap3isp/ispreg.h
+++ b/drivers/media/platform/omap3isp/ispreg.h
@@ -27,13 +27,13 @@
 #ifndef OMAP3_ISP_REG_H
 #define OMAP3_ISP_REG_H
 
-#include <plat/omap34xx.h>
-
-
 #define CM_CAM_MCLK_HZ                  172800000       /* Hz */
 
 /* ISP Submodules offset */
 
+#define L4_34XX_BASE                    0x48000000
+#define OMAP3430_ISP_BASE               (L4_34XX_BASE + 0xBC000)
+
 #define OMAP3ISP_REG_BASE               OMAP3430_ISP_BASE
 #define OMAP3ISP_REG(offset)            (OMAP3ISP_REG_BASE + (offset))
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-capture.c b/drivers/media/platform/s5p-fimc/fimc-capture.c
index dded98815220..367efd164d0f 100644
--- a/drivers/media/platform/s5p-fimc/fimc-capture.c
+++ b/drivers/media/platform/s5p-fimc/fimc-capture.c
@@ -177,7 +177,9 @@ static int fimc_capture_config_update(struct fimc_ctx *ctx)
 
 void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
 {
+        struct v4l2_subdev *csis = fimc->pipeline.subdevs[IDX_CSIS];
         struct fimc_vid_cap *cap = &fimc->vid_cap;
+        struct fimc_frame *f = &cap->ctx->d_frame;
         struct fimc_vid_buffer *v_buf;
         struct timeval *tv;
         struct timespec ts;
@@ -216,6 +218,25 @@ void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
                 if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
                         cap->buf_index = 0;
         }
+        /*
+         * Set up a buffer at MIPI-CSIS if current image format
+         * requires the frame embedded data capture.
+         */
+        if (f->fmt->mdataplanes && !list_empty(&cap->active_buf_q)) {
+                unsigned int plane = ffs(f->fmt->mdataplanes) - 1;
+                unsigned int size = f->payload[plane];
+                s32 index = fimc_hw_get_frame_index(fimc);
+                void *vaddr;
+
+                list_for_each_entry(v_buf, &cap->active_buf_q, list) {
+                        if (v_buf->index != index)
+                                continue;
+                        vaddr = vb2_plane_vaddr(&v_buf->vb, plane);
+                        v4l2_subdev_call(csis, video, s_rx_buffer,
+                                         vaddr, &size);
+                        break;
+                }
+        }
 
         if (cap->active_buf_cnt == 0) {
                 if (deq_buf)
@@ -351,6 +372,8 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
                 unsigned int size = (wh * fmt->depth[i]) / 8;
                 if (pixm)
                         sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+                else if (fimc_fmt_is_user_defined(fmt->color))
+                        sizes[i] = frame->payload[i];
                 else
                         sizes[i] = max_t(u32, size, frame->payload[i]);
 
@@ -611,10 +634,10 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
         u32 mask = FMT_FLAGS_CAM;
         struct fimc_fmt *ffmt;
 
-        /* Color conversion from/to JPEG is not supported */
+        /* Conversion from/to JPEG or User Defined format is not supported */
         if (code && ctx->s_frame.fmt && pad == FIMC_SD_PAD_SOURCE &&
-            fimc_fmt_is_jpeg(ctx->s_frame.fmt->color))
-                *code = V4L2_MBUS_FMT_JPEG_1X8;
+            fimc_fmt_is_user_defined(ctx->s_frame.fmt->color))
+                *code = ctx->s_frame.fmt->mbus_code;
 
         if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad != FIMC_SD_PAD_SINK)
                 mask |= FMT_FLAGS_M2M;
@@ -628,18 +651,19 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
                 *fourcc = ffmt->fourcc;
 
         if (pad == FIMC_SD_PAD_SINK) {
-                max_w = fimc_fmt_is_jpeg(ffmt->color) ?
+                max_w = fimc_fmt_is_user_defined(ffmt->color) ?
                         pl->scaler_dis_w : pl->scaler_en_w;
                 /* Apply the camera input interface pixel constraints */
                 v4l_bound_align_image(width, max_t(u32, *width, 32), max_w, 4,
                                       height, max_t(u32, *height, 32),
                                       FIMC_CAMIF_MAX_HEIGHT,
-                                      fimc_fmt_is_jpeg(ffmt->color) ? 3 : 1,
+                                      fimc_fmt_is_user_defined(ffmt->color) ?
+                                      3 : 1,
                                       0);
                 return ffmt;
         }
         /* Can't scale or crop in transparent (JPEG) transfer mode */
-        if (fimc_fmt_is_jpeg(ffmt->color)) {
+        if (fimc_fmt_is_user_defined(ffmt->color)) {
                 *width  = ctx->s_frame.f_width;
                 *height = ctx->s_frame.f_height;
                 return ffmt;
@@ -684,7 +708,7 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
         u32 max_sc_h, max_sc_v;
 
         /* In JPEG transparent transfer mode cropping is not supported */
-        if (fimc_fmt_is_jpeg(ctx->d_frame.fmt->color)) {
+        if (fimc_fmt_is_user_defined(ctx->d_frame.fmt->color)) {
                 r->width  = sink->f_width;
                 r->height = sink->f_height;
                 r->left   = r->top = 0;
@@ -847,6 +871,48 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
         return 0;
 }
 
+/**
+ * fimc_get_sensor_frame_desc - query the sensor for media bus frame parameters
+ * @sensor: pointer to the sensor subdev
+ * @plane_fmt: provides plane sizes corresponding to the frame layout entries
+ * @try: true to set the frame parameters, false to query only
+ *
+ * This function is used by this driver only for compressed/blob data formats.
+ */
+static int fimc_get_sensor_frame_desc(struct v4l2_subdev *sensor,
+                                      struct v4l2_plane_pix_format *plane_fmt,
+                                      unsigned int num_planes, bool try)
+{
+        struct v4l2_mbus_frame_desc fd;
+        int i, ret;
+
+        for (i = 0; i < num_planes; i++)
+                fd.entry[i].length = plane_fmt[i].sizeimage;
+
+        if (try)
+                ret = v4l2_subdev_call(sensor, pad, set_frame_desc, 0, &fd);
+        else
+                ret = v4l2_subdev_call(sensor, pad, get_frame_desc, 0, &fd);
+
+        if (ret < 0)
+                return ret;
+
+        if (num_planes != fd.num_entries)
+                return -EINVAL;
+
+        for (i = 0; i < num_planes; i++)
+                plane_fmt[i].sizeimage = fd.entry[i].length;
+
+        if (fd.entry[0].length > FIMC_MAX_JPEG_BUF_SIZE) {
+                v4l2_err(sensor->v4l2_dev,  "Unsupported buffer size: %u\n",
+                         fd.entry[0].length);
+
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
 static int fimc_cap_g_fmt_mplane(struct file *file, void *fh,
                                  struct v4l2_format *f)
 {
@@ -865,7 +931,7 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
         struct v4l2_mbus_framefmt mf;
         struct fimc_fmt *ffmt = NULL;
 
-        if (pix->pixelformat == V4L2_PIX_FMT_JPEG) {
+        if (fimc_jpeg_fourcc(pix->pixelformat)) {
                 fimc_capture_try_format(ctx, &pix->width, &pix->height,
                                         NULL, &pix->pixelformat,
                                         FIMC_SD_PAD_SINK);
@@ -879,25 +945,32 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
                 return -EINVAL;
 
         if (!fimc->vid_cap.user_subdev_api) {
-                mf.width  = pix->width;
+                mf.width = pix->width;
                 mf.height = pix->height;
-                mf.code   = ffmt->mbus_code;
+                mf.code = ffmt->mbus_code;
                 fimc_md_graph_lock(fimc);
                 fimc_pipeline_try_format(ctx, &mf, &ffmt, false);
                 fimc_md_graph_unlock(fimc);
-
-                pix->width       = mf.width;
-                pix->height      = mf.height;
+                pix->width = mf.width;
+                pix->height = mf.height;
                 if (ffmt)
                         pix->pixelformat = ffmt->fourcc;
         }
 
         fimc_adjust_mplane_format(ffmt, pix->width, pix->height, pix);
+
+        if (ffmt->flags & FMT_FLAGS_COMPRESSED)
+                fimc_get_sensor_frame_desc(fimc->pipeline.subdevs[IDX_SENSOR],
+                                        pix->plane_fmt, ffmt->memplanes, true);
+
         return 0;
 }
 
-static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx, bool jpeg)
+static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx,
+                                        enum fimc_color_fmt color)
 {
+        bool jpeg = fimc_fmt_is_user_defined(color);
+
         ctx->scaler.enabled = !jpeg;
         fimc_ctrls_activate(ctx, !jpeg);
 
@@ -920,7 +993,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
                 return -EBUSY;
 
         /* Pre-configure format at camera interface input, for JPEG only */
-        if (pix->pixelformat == V4L2_PIX_FMT_JPEG) {
+        if (fimc_jpeg_fourcc(pix->pixelformat)) {
                 fimc_capture_try_format(ctx, &pix->width, &pix->height,
                                         NULL, &pix->pixelformat,
                                         FIMC_SD_PAD_SINK);
@@ -953,7 +1026,16 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
         }
 
         fimc_adjust_mplane_format(ff->fmt, pix->width, pix->height, pix);
-        for (i = 0; i < ff->fmt->colplanes; i++)
+
+        if (ff->fmt->flags & FMT_FLAGS_COMPRESSED) {
+                ret = fimc_get_sensor_frame_desc(fimc->pipeline.subdevs[IDX_SENSOR],
+                                        pix->plane_fmt, ff->fmt->memplanes,
+                                        true);
+                if (ret < 0)
+                        return ret;
+        }
+
+        for (i = 0; i < ff->fmt->memplanes; i++)
                 ff->payload[i] = pix->plane_fmt[i].sizeimage;
 
         set_frame_bounds(ff, pix->width, pix->height);
@@ -961,7 +1043,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
         if (!(ctx->state & FIMC_COMPOSE))
                 set_frame_crop(ff, 0, 0, pix->width, pix->height);
 
-        fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ff->fmt->color));
+        fimc_capture_mark_jpeg_xfer(ctx, ff->fmt->color);
 
         /* Reset cropping and set format at the camera interface input */
         if (!fimc->vid_cap.user_subdev_api) {
@@ -1063,6 +1145,23 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
                     src_fmt.format.height != sink_fmt.format.height ||
                     src_fmt.format.code != sink_fmt.format.code)
                         return -EPIPE;
+
+                if (sd == fimc->pipeline.subdevs[IDX_SENSOR] &&
+                    fimc_user_defined_mbus_fmt(src_fmt.format.code)) {
+                        struct v4l2_plane_pix_format plane_fmt[FIMC_MAX_PLANES];
+                        struct fimc_frame *frame = &vid_cap->ctx->d_frame;
+                        unsigned int i;
+
+                        ret = fimc_get_sensor_frame_desc(sd, plane_fmt,
+                                                         frame->fmt->memplanes,
+                                                         false);
+                        if (ret < 0)
+                                return -EPIPE;
+
+                        for (i = 0; i < frame->fmt->memplanes; i++)
+                                if (frame->payload[i] < plane_fmt[i].sizeimage)
+                                        return -EPIPE;
+                }
         }
         return 0;
 }
@@ -1424,7 +1523,7 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
         /* Update RGB Alpha control state and value range */
         fimc_alpha_ctrl_update(ctx);
 
-        fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ffmt->color));
+        fimc_capture_mark_jpeg_xfer(ctx, ffmt->color);
 
         ff = fmt->pad == FIMC_SD_PAD_SINK ?
                 &ctx->s_frame : &ctx->d_frame;
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.c b/drivers/media/platform/s5p-fimc/fimc-core.c
index 1a445404e73d..8d0d2b94a135 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.c
+++ b/drivers/media/platform/s5p-fimc/fimc-core.c
@@ -184,7 +184,17 @@ static struct fimc_fmt fimc_formats[] = {
                 .memplanes      = 1,
                 .colplanes      = 1,
                 .mbus_code      = V4L2_MBUS_FMT_JPEG_1X8,
-                .flags          = FMT_FLAGS_CAM,
+                .flags          = FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
+        }, {
+                .name           = "S5C73MX interleaved UYVY/JPEG",
+                .fourcc         = V4L2_PIX_FMT_S5C_UYVY_JPG,
+                .color          = FIMC_FMT_YUYV_JPEG,
+                .depth          = { 8 },
+                .memplanes      = 2,
+                .colplanes      = 1,
+                .mdataplanes    = 0x2, /* plane 1 holds frame meta data */
+                .mbus_code      = V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
+                .flags          = FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
         },
 };
 
@@ -371,7 +381,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
                 default:
                         return -EINVAL;
                 }
-        } else {
+        } else if (!frame->fmt->mdataplanes) {
                 if (frame->fmt->memplanes >= 2)
                         paddr->cb = vb2_dma_contig_plane_dma_addr(vb, 1);
 
@@ -698,6 +708,11 @@ int fimc_fill_format(struct fimc_frame *frame, struct v4l2_format *f)
                 if (frame->fmt->colplanes == 1) /* packed formats */
                         bpl = (bpl * frame->fmt->depth[0]) / 8;
                 pixm->plane_fmt[i].bytesperline = bpl;
+
+                if (frame->fmt->flags & FMT_FLAGS_COMPRESSED) {
+                        pixm->plane_fmt[i].sizeimage = frame->payload[i];
+                        continue;
+                }
                 pixm->plane_fmt[i].sizeimage = (frame->o_width *
                         frame->o_height * frame->fmt->depth[i]) / 8;
         }
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.h b/drivers/media/platform/s5p-fimc/fimc-core.h
index cd716ba6015f..c0040d792499 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.h
+++ b/drivers/media/platform/s5p-fimc/fimc-core.h
@@ -40,6 +40,8 @@
 #define SCALER_MAX_VRATIO       64
 #define DMA_MIN_SIZE            8
 #define FIMC_CAMIF_MAX_HEIGHT   0x2000
+#define FIMC_MAX_JPEG_BUF_SIZE  (10 * SZ_1M)
+#define FIMC_MAX_PLANES         3
 
 /* indices to the clocks array */
 enum {
@@ -83,7 +85,7 @@ enum fimc_datapath {
 };
 
 enum fimc_color_fmt {
-        FIMC_FMT_RGB444 = 0x10,
+        FIMC_FMT_RGB444 = 0x10,
         FIMC_FMT_RGB555,
         FIMC_FMT_RGB565,
         FIMC_FMT_RGB666,
@@ -95,14 +97,15 @@ enum fimc_color_fmt {
         FIMC_FMT_CBYCRY422,
         FIMC_FMT_CRYCBY422,
         FIMC_FMT_YCBCR444_LOCAL,
-        FIMC_FMT_JPEG = 0x40,
-        FIMC_FMT_RAW8 = 0x80,
+        FIMC_FMT_RAW8 = 0x40,
         FIMC_FMT_RAW10,
         FIMC_FMT_RAW12,
+        FIMC_FMT_JPEG = 0x80,
+        FIMC_FMT_YUYV_JPEG = 0x100,
 };
 
+#define fimc_fmt_is_user_defined(x) (!!((x) & 0x180))
 #define fimc_fmt_is_rgb(x) (!!((x) & 0x10))
-#define fimc_fmt_is_jpeg(x) (!!((x) & 0x40))
 
 #define IS_M2M(__strt) ((__strt) == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE || \
                         __strt == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
@@ -139,6 +142,7 @@ enum fimc_color_fmt {
  * @memplanes: number of physically non-contiguous data planes
  * @colplanes: number of physically contiguous data planes
  * @depth: per plane driver's private 'number of bits per pixel'
+ * @mdataplanes: bitmask indicating meta data plane(s), (1 << plane_no)
  * @flags: flags indicating which operation mode format applies to
  */
 struct fimc_fmt {
@@ -149,12 +153,14 @@ struct fimc_fmt {
         u16     memplanes;
         u16     colplanes;
         u8      depth[VIDEO_MAX_PLANES];
+        u16     mdataplanes;
         u16     flags;
 #define FMT_FLAGS_CAM           (1 << 0)
 #define FMT_FLAGS_M2M_IN        (1 << 1)
 #define FMT_FLAGS_M2M_OUT       (1 << 2)
 #define FMT_FLAGS_M2M           (1 << 1 | 1 << 2)
 #define FMT_HAS_ALPHA           (1 << 3)
+#define FMT_FLAGS_COMPRESSED    (1 << 4)
 };
 
 /**
@@ -272,7 +278,7 @@ struct fimc_frame {
         u32     offs_v;
         u32     width;
         u32     height;
-        unsigned long           payload[VIDEO_MAX_PLANES];
+        unsigned int            payload[VIDEO_MAX_PLANES];
         struct fimc_addr        paddr;
         struct fimc_dma_offset  dma_offset;
         struct fimc_fmt         *fmt;
@@ -577,6 +583,18 @@ static inline int tiled_fmt(struct fimc_fmt *fmt)
         return fmt->fourcc == V4L2_PIX_FMT_NV12MT;
 }
 
+static inline bool fimc_jpeg_fourcc(u32 pixelformat)
+{
+        return (pixelformat == V4L2_PIX_FMT_JPEG ||
+                pixelformat == V4L2_PIX_FMT_S5C_UYVY_JPG);
+}
+
+static inline bool fimc_user_defined_mbus_fmt(u32 code)
+{
+        return (code == V4L2_MBUS_FMT_JPEG_1X8 ||
+                code == V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8);
+}
+
 /* Return the alpha component bit mask */
 static inline int fimc_get_alpha_mask(struct fimc_fmt *fmt)
 {
diff --git a/drivers/media/platform/s5p-fimc/fimc-m2m.c b/drivers/media/platform/s5p-fimc/fimc-m2m.c
index 6b71d953fd15..4500e44f6857 100644
--- a/drivers/media/platform/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/platform/s5p-fimc/fimc-m2m.c
@@ -551,30 +551,31 @@ static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
         return 0;
 }
 
-static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr)
+static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
 {
         struct fimc_ctx *ctx = fh_to_ctx(fh);
         struct fimc_dev *fimc = ctx->fimc_dev;
+        struct v4l2_crop cr = *crop;
         struct fimc_frame *f;
         int ret;
 
-        ret = fimc_m2m_try_crop(ctx, cr);
+        ret = fimc_m2m_try_crop(ctx, &cr);
         if (ret)
                 return ret;
 
-        f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
+        f = (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
                 &ctx->s_frame : &ctx->d_frame;
 
         /* Check to see if scaling ratio is within supported range */
         if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
-                if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-                        ret = fimc_check_scaler_ratio(ctx, cr->c.width,
-                                        cr->c.height, ctx->d_frame.width,
+                if (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                        ret = fimc_check_scaler_ratio(ctx, cr.c.width,
+                                        cr.c.height, ctx->d_frame.width,
                                         ctx->d_frame.height, ctx->rotation);
                 } else {
                         ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
-                                        ctx->s_frame.height, cr->c.width,
-                                        cr->c.height, ctx->rotation);
+                                        ctx->s_frame.height, cr.c.width,
+                                        cr.c.height, ctx->rotation);
                 }
                 if (ret) {
                         v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
@@ -582,10 +583,10 @@ static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *
                 }
         }
 
-        f->offs_h = cr->c.left;
-        f->offs_v = cr->c.top;
-        f->width  = cr->c.width;
-        f->height = cr->c.height;
+        f->offs_h = cr.c.left;
+        f->offs_v = cr.c.top;
+        f->width  = cr.c.width;
+        f->height = cr.c.height;
 
         fimc_ctx_state_set(FIMC_PARAMS, ctx);
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.c b/drivers/media/platform/s5p-fimc/fimc-reg.c
index 783408fd7d56..2c9d0c06c9e8 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.c
+++ b/drivers/media/platform/s5p-fimc/fimc-reg.c
@@ -625,7 +625,7 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
                                 cfg |= FIMC_REG_CISRCFMT_ITU601_16BIT;
                 } /* else defaults to ITU-R BT.656 8-bit */
         } else if (cam->bus_type == FIMC_MIPI_CSI2) {
-                if (fimc_fmt_is_jpeg(f->fmt->color))
+                if (fimc_fmt_is_user_defined(f->fmt->color))
                         cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
         }
 
@@ -680,6 +680,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
                         tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
                         break;
                 case V4L2_MBUS_FMT_JPEG_1X8:
+                case V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8:
                         tmp = FIMC_REG_CSIIMGFMT_USER(1);
                         cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
                         break;
@@ -744,13 +745,13 @@ void fimc_hw_dis_capture(struct fimc_dev *dev)
 }
 
 /* Return an index to the buffer actually being written. */
-u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev)
 {
-        u32 reg;
+        s32 reg;
 
         if (dev->variant->has_cistatus2) {
-                reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3F;
-                return reg > 0 ? --reg : reg;
+                reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3f;
+                return reg - 1;
         }
 
         reg = readl(dev->regs + FIMC_REG_CISTATUS);
@@ -759,6 +760,18 @@ u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
                 FIMC_REG_CISTATUS_FRAMECNT_SHIFT;
 }
 
+/* Return an index to the buffer being written previously. */
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev)
+{
+        s32 reg;
+
+        if (!dev->variant->has_cistatus2)
+                return -1;
+
+        reg = readl(dev->regs + FIMC_REG_CISTATUS2);
+        return ((reg >> 7) & 0x3f) - 1;
+}
+
 /* Locking: the caller holds fimc->slock */
 void fimc_activate_capture(struct fimc_ctx *ctx)
 {
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.h b/drivers/media/platform/s5p-fimc/fimc-reg.h
index 579ac8ac03de..b6abfc7b72ac 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.h
+++ b/drivers/media/platform/s5p-fimc/fimc-reg.h
@@ -307,7 +307,8 @@ void fimc_hw_clear_irq(struct fimc_dev *dev);
 void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on);
 void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
 void fimc_hw_dis_capture(struct fimc_dev *dev);
-u32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev);
 void fimc_activate_capture(struct fimc_ctx *ctx);
 void fimc_deactivate_capture(struct fimc_dev *fimc);
 
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.c b/drivers/media/platform/s5p-fimc/mipi-csis.c
index e92236ac5cfe..4c961b1b68e6 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.c
+++ b/drivers/media/platform/s5p-fimc/mipi-csis.c
@@ -2,7 +2,7 @@
  * Samsung S5P/EXYNOS4 SoC series MIPI-CSI receiver driver
  *
  * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
- * Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -98,6 +98,11 @@ MODULE_PARM_DESC(debug, "Debug level (0-2)");
 #define CSIS_MAX_PIX_WIDTH              0xffff
 #define CSIS_MAX_PIX_HEIGHT             0xffff
 
+/* Non-image packet data buffers */
+#define S5PCSIS_PKTDATA_ODD             0x2000
+#define S5PCSIS_PKTDATA_EVEN            0x3000
+#define S5PCSIS_PKTDATA_SIZE            SZ_4K
+
 enum {
         CSIS_CLK_MUX,
         CSIS_CLK_GATE,
@@ -110,8 +115,8 @@ static char *csi_clock_name[] = {
 #define NUM_CSIS_CLOCKS ARRAY_SIZE(csi_clock_name)
 
 static const char * const csis_supply_name[] = {
-        "vdd11", /* 1.1V or 1.2V (s5pc100) MIPI CSI suppply */
-        "vdd18", /* VDD 1.8V and MIPI CSI PLL supply */
+        "vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
+        "vddio",    /* CSIS I/O and PLL (1.8V) supply */
 };
 #define CSIS_NUM_SUPPLIES ARRAY_SIZE(csis_supply_name)
 
@@ -144,12 +149,18 @@ static const struct s5pcsis_event s5pcsis_events[] = {
 };
 #define S5PCSIS_NUM_EVENTS ARRAY_SIZE(s5pcsis_events)
 
+struct csis_pktbuf {
+        u32 *data;
+        unsigned int len;
+};
+
 /**
  * struct csis_state - the driver's internal state data structure
  * @lock: mutex serializing the subdev and power management operations,
  *        protecting @format and @flags members
  * @pads: CSIS pads array
  * @sd: v4l2_subdev associated with CSIS device instance
+ * @index: the hardware instance index
  * @pdev: CSIS platform device
  * @regs: mmaped I/O registers memory
  * @supplies: CSIS regulator supplies
@@ -159,12 +170,14 @@ static const struct s5pcsis_event s5pcsis_events[] = {
  * @csis_fmt: current CSIS pixel format
  * @format: common media bus format for the source and sink pad
  * @slock: spinlock protecting structure members below
+ * @pkt_buf: the frame embedded (non-image) data buffer
  * @events: MIPI-CSIS event (error) counters
  */
 struct csis_state {
         struct mutex lock;
         struct media_pad pads[CSIS_PADS_NUM];
         struct v4l2_subdev sd;
+        u8 index;
         struct platform_device *pdev;
         void __iomem *regs;
         struct regulator_bulk_data supplies[CSIS_NUM_SUPPLIES];
@@ -175,6 +188,7 @@ struct csis_state {
         struct v4l2_mbus_framefmt format;
 
         struct spinlock slock;
+        struct csis_pktbuf pkt_buf;
         struct s5pcsis_event events[S5PCSIS_NUM_EVENTS];
 };
 
@@ -202,7 +216,11 @@ static const struct csis_pix_format s5pcsis_formats[] = {
                 .code = V4L2_MBUS_FMT_JPEG_1X8,
                 .fmt_reg = S5PCSIS_CFG_FMT_USER(1),
                 .data_alignment = 32,
-        },
+        }, {
+                .code = V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
+                .fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+                .data_alignment = 32,
+        }
 };
 
 #define s5pcsis_write(__csis, __r, __v) writel(__v, __csis->regs + __r)
@@ -266,7 +284,7 @@ static void __s5pcsis_set_format(struct csis_state *state)
         struct v4l2_mbus_framefmt *mf = &state->format;
         u32 val;
 
-        v4l2_dbg(1, debug, &state->sd, "fmt: %d, %d x %d\n",
+        v4l2_dbg(1, debug, &state->sd, "fmt: %#x, %d x %d\n",
                  mf->code, mf->width, mf->height);
 
         /* Color format */
@@ -304,8 +322,10 @@ static void s5pcsis_set_params(struct csis_state *state)
                 val |= S5PCSIS_CTRL_ALIGN_32BIT;
         else /* 24-bits */
                 val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
-        /* Not using external clock. */
+
         val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
+        if (pdata->wclk_source)
+                val |= S5PCSIS_CTRL_WCLK_EXTCLK;
         s5pcsis_write(state, S5PCSIS_CTRL, val);
 
         /* Update the shadow register. */
@@ -529,6 +549,22 @@ static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
         return 0;
 }
 
+static int s5pcsis_s_rx_buffer(struct v4l2_subdev *sd, void *buf,
+                               unsigned int *size)
+{
+        struct csis_state *state = sd_to_csis_state(sd);
+        unsigned long flags;
+
+        *size = min_t(unsigned int, *size, S5PCSIS_PKTDATA_SIZE);
+
+        spin_lock_irqsave(&state->slock, flags);
+        state->pkt_buf.data = buf;
+        state->pkt_buf.len = *size;
+        spin_unlock_irqrestore(&state->slock, flags);
+
+        return 0;
+}
+
 static int s5pcsis_log_status(struct v4l2_subdev *sd)
 {
         struct csis_state *state = sd_to_csis_state(sd);
@@ -566,6 +602,7 @@ static struct v4l2_subdev_pad_ops s5pcsis_pad_ops = {
 };
 
 static struct v4l2_subdev_video_ops s5pcsis_video_ops = {
+        .s_rx_buffer = s5pcsis_s_rx_buffer,
         .s_stream = s5pcsis_s_stream,
 };
 
@@ -578,13 +615,26 @@ static struct v4l2_subdev_ops s5pcsis_subdev_ops = {
 static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 {
         struct csis_state *state = dev_id;
+        struct csis_pktbuf *pktbuf = &state->pkt_buf;
         unsigned long flags;
         u32 status;
 
         status = s5pcsis_read(state, S5PCSIS_INTSRC);
-
         spin_lock_irqsave(&state->slock, flags);
 
+        if ((status & S5PCSIS_INTSRC_NON_IMAGE_DATA) && pktbuf->data) {
+                u32 offset;
+
+                if (status & S5PCSIS_INTSRC_EVEN)
+                        offset = S5PCSIS_PKTDATA_EVEN;
+                else
+                        offset = S5PCSIS_PKTDATA_ODD;
+
+                memcpy(pktbuf->data, state->regs + offset, pktbuf->len);
+                pktbuf->data = NULL;
+                rmb();
+        }
+
         /* Update the event/error counters */
         if ((status & S5PCSIS_INTSRC_ERRORS) || debug) {
                 int i;
@@ -620,14 +670,15 @@ static int __devinit s5pcsis_probe(struct platform_device *pdev)
         spin_lock_init(&state->slock);
 
         state->pdev = pdev;
+        state->index = max(0, pdev->id);
 
         pdata = pdev->dev.platform_data;
-        if (pdata == NULL || pdata->phy_enable == NULL) {
+        if (pdata == NULL) {
                 dev_err(&pdev->dev, "Platform data not fully specified\n");
                 return -EINVAL;
         }
 
-        if ((pdev->id == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
+        if ((state->index == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
             pdata->lanes > CSIS0_MAX_LANES) {
                 dev_err(&pdev->dev, "Unsupported number of data lanes: %d\n",
                         pdata->lanes);
@@ -710,7 +761,6 @@ e_clkput:
 
 static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 {
-        struct s5p_platform_mipi_csis *pdata = dev->platform_data;
         struct platform_device *pdev = to_platform_device(dev);
         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
         struct csis_state *state = sd_to_csis_state(sd);
@@ -722,7 +772,7 @@ static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
         mutex_lock(&state->lock);
         if (state->flags & ST_POWERED) {
                 s5pcsis_stop_stream(state);
-                ret = pdata->phy_enable(state->pdev, false);
+                ret = s5p_csis_phy_enable(state->index, false);
                 if (ret)
                         goto unlock;
                 ret = regulator_bulk_disable(CSIS_NUM_SUPPLIES,
@@ -741,7 +791,6 @@ static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 
 static int s5pcsis_pm_resume(struct device *dev, bool runtime)
 {
-        struct s5p_platform_mipi_csis *pdata = dev->platform_data;
         struct platform_device *pdev = to_platform_device(dev);
         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
         struct csis_state *state = sd_to_csis_state(sd);
@@ -759,7 +808,7 @@ static int s5pcsis_pm_resume(struct device *dev, bool runtime)
                                             state->supplies);
                 if (ret)
                         goto unlock;
-                ret = pdata->phy_enable(state->pdev, true);
+                ret = s5p_csis_phy_enable(state->index, true);
                 if (!ret) {
                         state->flags |= ST_POWERED;
                 } else {
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 1e3b9dd014c0..1bfbc325836b 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -507,7 +507,7 @@ static int vidioc_g_crop(struct file *file, void *prv, struct v4l2_crop *cr)
         return 0;
 }
 
-static int vidioc_try_crop(struct file *file, void *prv, struct v4l2_crop *cr)
+static int vidioc_try_crop(struct file *file, void *prv, const struct v4l2_crop *cr)
 {
         struct g2d_ctx *ctx = prv;
         struct g2d_dev *dev = ctx->dev;
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 394775ae5774..17983c4c9a9a 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1353,7 +1353,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
                 return ret;
         }
         dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
-        clk_enable(jpeg->clk);
+        clk_prepare_enable(jpeg->clk);
 
         /* v4l2 device */
         ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
@@ -1460,7 +1460,7 @@ device_register_rollback:
         v4l2_device_unregister(&jpeg->v4l2_dev);
 
 clk_get_rollback:
-        clk_disable(jpeg->clk);
+        clk_disable_unprepare(jpeg->clk);
         clk_put(jpeg->clk);
 
         return ret;
@@ -1480,7 +1480,7 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
         v4l2_m2m_release(jpeg->m2m_dev);
         v4l2_device_unregister(&jpeg->v4l2_dev);
 
-        clk_disable(jpeg->clk);
+        clk_disable_unprepare(jpeg->clk);
         clk_put(jpeg->clk);
 
         return 0;
diff --git a/drivers/media/platform/s5p-mfc/Makefile b/drivers/media/platform/s5p-mfc/Makefile
index d0663409af00..379008c6d09a 100644
--- a/drivers/media/platform/s5p-mfc/Makefile
+++ b/drivers/media/platform/s5p-mfc/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC) := s5p-mfc.o
-s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o s5p_mfc_opr.o
+s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o
 s5p-mfc-y += s5p_mfc_dec.o s5p_mfc_enc.o
-s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_cmd.o
-s5p-mfc-y += s5p_mfc_pm.o s5p_mfc_shm.o
+s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_pm.o
+s5p-mfc-y += s5p_mfc_opr.o s5p_mfc_opr_v5.o s5p_mfc_opr_v6.o
+s5p-mfc-y += s5p_mfc_cmd.o s5p_mfc_cmd_v5.o s5p_mfc_cmd_v6.o
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
new file mode 100644
index 000000000000..363a97cc7681
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
@@ -0,0 +1,408 @@
+/*
+ * Register definition file for Samsung MFC V6.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _REGS_FIMV_V6_H
+#define _REGS_FIMV_V6_H
+
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
+#define S5P_FIMV_REG_SIZE_V6    (S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
+#define S5P_FIMV_REG_COUNT_V6   ((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
+
+/* Number of bits that the buffer address should be shifted for particular
+ * MFC buffers.  */
+#define S5P_FIMV_MEM_OFFSET_V6          0
+
+#define S5P_FIMV_START_ADDR_V6          0x0000
+#define S5P_FIMV_END_ADDR_V6            0xfd80
+
+#define S5P_FIMV_REG_CLEAR_BEGIN_V6     0xf000
+#define S5P_FIMV_REG_CLEAR_COUNT_V6     1024
+
+/* Codec Common Registers */
+#define S5P_FIMV_RISC_ON_V6                     0x0000
+#define S5P_FIMV_RISC2HOST_INT_V6               0x003C
+#define S5P_FIMV_HOST2RISC_INT_V6               0x0044
+#define S5P_FIMV_RISC_BASE_ADDRESS_V6           0x0054
+
+#define S5P_FIMV_MFC_RESET_V6                   0x1070
+
+#define S5P_FIMV_HOST2RISC_CMD_V6               0x1100
+#define S5P_FIMV_H2R_CMD_EMPTY_V6               0
+#define S5P_FIMV_H2R_CMD_SYS_INIT_V6            1
+#define S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6       2
+#define S5P_FIMV_CH_SEQ_HEADER_V6               3
+#define S5P_FIMV_CH_INIT_BUFS_V6                4
+#define S5P_FIMV_CH_FRAME_START_V6              5
+#define S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6      6
+#define S5P_FIMV_H2R_CMD_SLEEP_V6               7
+#define S5P_FIMV_H2R_CMD_WAKEUP_V6              8
+#define S5P_FIMV_CH_LAST_FRAME_V6               9
+#define S5P_FIMV_H2R_CMD_FLUSH_V6               10
+/* RMVME: REALLOC used? */
+#define S5P_FIMV_CH_FRAME_START_REALLOC_V6      5
+
+#define S5P_FIMV_RISC2HOST_CMD_V6               0x1104
+#define S5P_FIMV_R2H_CMD_EMPTY_V6               0
+#define S5P_FIMV_R2H_CMD_SYS_INIT_RET_V6        1
+#define S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET_V6   2
+#define S5P_FIMV_R2H_CMD_SEQ_DONE_RET_V6        3
+#define S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET_V6    4
+
+#define S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET_V6  6
+#define S5P_FIMV_R2H_CMD_SLEEP_RET_V6           7
+#define S5P_FIMV_R2H_CMD_WAKEUP_RET_V6          8
+#define S5P_FIMV_R2H_CMD_COMPLETE_SEQ_RET_V6    9
+#define S5P_FIMV_R2H_CMD_DPB_FLUSH_RET_V6       10
+#define S5P_FIMV_R2H_CMD_NAL_ABORT_RET_V6       11
+#define S5P_FIMV_R2H_CMD_FW_STATUS_RET_V6       12
+#define S5P_FIMV_R2H_CMD_FRAME_DONE_RET_V6      13
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET_V6      14
+#define S5P_FIMV_R2H_CMD_SLICE_DONE_RET_V6      15
+#define S5P_FIMV_R2H_CMD_ENC_BUFFER_FUL_RET_V6  16
+#define S5P_FIMV_R2H_CMD_ERR_RET_V6             32
+
+#define S5P_FIMV_FW_VERSION_V6                  0xf000
+
+#define S5P_FIMV_INSTANCE_ID_V6                 0xf008
+#define S5P_FIMV_CODEC_TYPE_V6                  0xf00c
+#define S5P_FIMV_CONTEXT_MEM_ADDR_V6            0xf014
+#define S5P_FIMV_CONTEXT_MEM_SIZE_V6            0xf018
+#define S5P_FIMV_PIXEL_FORMAT_V6                0xf020
+
+#define S5P_FIMV_METADATA_ENABLE_V6             0xf024
+#define S5P_FIMV_DBG_BUFFER_ADDR_V6             0xf030
+#define S5P_FIMV_DBG_BUFFER_SIZE_V6             0xf034
+#define S5P_FIMV_RET_INSTANCE_ID_V6             0xf070
+
+#define S5P_FIMV_ERROR_CODE_V6                  0xf074
+#define S5P_FIMV_ERR_WARNINGS_START_V6          160
+#define S5P_FIMV_ERR_DEC_MASK_V6                0xffff
+#define S5P_FIMV_ERR_DEC_SHIFT_V6               0
+#define S5P_FIMV_ERR_DSPL_MASK_V6               0xffff0000
+#define S5P_FIMV_ERR_DSPL_SHIFT_V6              16
+
+#define S5P_FIMV_DBG_BUFFER_OUTPUT_SIZE_V6      0xf078
+#define S5P_FIMV_METADATA_STATUS_V6             0xf07C
+#define S5P_FIMV_METADATA_ADDR_MB_INFO_V6       0xf080
+#define S5P_FIMV_METADATA_SIZE_MB_INFO_V6       0xf084
+
+/* Decoder Registers */
+#define S5P_FIMV_D_CRC_CTRL_V6                  0xf0b0
+#define S5P_FIMV_D_DEC_OPTIONS_V6               0xf0b4
+#define S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6     4
+#define S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6       3
+#define S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6         1
+#define S5P_FIMV_D_OPT_LF_CTRL_MASK_V6          0x3
+#define S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6       0
+
+#define S5P_FIMV_D_DISPLAY_DELAY_V6             0xf0b8
+
+#define S5P_FIMV_D_SET_FRAME_WIDTH_V6           0xf0bc
+#define S5P_FIMV_D_SET_FRAME_HEIGHT_V6          0xf0c0
+
+#define S5P_FIMV_D_SEI_ENABLE_V6                0xf0c4
+
+/* Buffer setting registers */
+#define S5P_FIMV_D_MIN_NUM_DPB_V6               0xf0f0
+#define S5P_FIMV_D_MIN_LUMA_DPB_SIZE_V6         0xf0f4
+#define S5P_FIMV_D_MIN_CHROMA_DPB_SIZE_V6       0xf0f8
+#define S5P_FIMV_D_MVC_NUM_VIEWS_V6             0xf0fc
+#define S5P_FIMV_D_MIN_NUM_MV_V6                0xf100
+#define S5P_FIMV_D_NUM_DPB_V6                   0xf130
+#define S5P_FIMV_D_LUMA_DPB_SIZE_V6             0xf134
+#define S5P_FIMV_D_CHROMA_DPB_SIZE_V6           0xf138
+#define S5P_FIMV_D_MV_BUFFER_SIZE_V6            0xf13c
+
+#define S5P_FIMV_D_LUMA_DPB_V6                  0xf140
+#define S5P_FIMV_D_CHROMA_DPB_V6                0xf240
+#define S5P_FIMV_D_MV_BUFFER_V6                 0xf340
+
+#define S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6       0xf440
+#define S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6       0xf444
+#define S5P_FIMV_D_METADATA_BUFFER_ADDR_V6      0xf448
+#define S5P_FIMV_D_METADATA_BUFFER_SIZE_V6      0xf44c
+#define S5P_FIMV_D_NUM_MV_V6                    0xf478
+#define S5P_FIMV_D_CPB_BUFFER_ADDR_V6           0xf4b0
+#define S5P_FIMV_D_CPB_BUFFER_SIZE_V6           0xf4b4
+
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_UPPER_V6  0xf4b8
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6  0xf4bc
+#define S5P_FIMV_D_CPB_BUFFER_OFFSET_V6         0xf4c0
+#define S5P_FIMV_D_SLICE_IF_ENABLE_V6           0xf4c4
+#define S5P_FIMV_D_PICTURE_TAG_V6               0xf4c8
+#define S5P_FIMV_D_STREAM_DATA_SIZE_V6          0xf4d0
+
+/* Display information register */
+#define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6       0xf500
+#define S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6      0xf504
+
+/* Display status */
+#define S5P_FIMV_D_DISPLAY_STATUS_V6            0xf508
+
+#define S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6         0xf50c
+#define S5P_FIMV_D_DISPLAY_CHROMA_ADDR_V6       0xf510
+
+#define S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6        0xf514
+
+#define S5P_FIMV_D_DISPLAY_CROP_INFO1_V6        0xf518
+#define S5P_FIMV_D_DISPLAY_CROP_INFO2_V6        0xf51c
+#define S5P_FIMV_D_DISPLAY_PICTURE_PROFILE_V6   0xf520
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_TOP_V6      0xf524
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_TOP_V6    0xf528
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_BOT_V6      0xf52c
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_BOT_V6    0xf530
+#define S5P_FIMV_D_DISPLAY_ASPECT_RATIO_V6      0xf534
+#define S5P_FIMV_D_DISPLAY_EXTENDED_AR_V6       0xf538
+
+/* Decoded picture information register */
+#define S5P_FIMV_D_DECODED_FRAME_WIDTH_V6       0xf53c
+#define S5P_FIMV_D_DECODED_FRAME_HEIGHT_V6      0xf540
+#define S5P_FIMV_D_DECODED_STATUS_V6            0xf544
+#define S5P_FIMV_DEC_CRC_GEN_MASK_V6            0x1
+#define S5P_FIMV_DEC_CRC_GEN_SHIFT_V6           6
+
+#define S5P_FIMV_D_DECODED_LUMA_ADDR_V6         0xf548
+#define S5P_FIMV_D_DECODED_CHROMA_ADDR_V6       0xf54c
+
+#define S5P_FIMV_D_DECODED_FRAME_TYPE_V6        0xf550
+#define S5P_FIMV_DECODE_FRAME_MASK_V6           7
+
+#define S5P_FIMV_D_DECODED_CROP_INFO1_V6        0xf554
+#define S5P_FIMV_D_DECODED_CROP_INFO2_V6        0xf558
+#define S5P_FIMV_D_DECODED_PICTURE_PROFILE_V6   0xf55c
+#define S5P_FIMV_D_DECODED_NAL_SIZE_V6          0xf560
+#define S5P_FIMV_D_DECODED_LUMA_CRC_TOP_V6      0xf564
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_TOP_V6    0xf568
+#define S5P_FIMV_D_DECODED_LUMA_CRC_BOT_V6      0xf56c
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_BOT_V6    0xf570
+
+/* Returned value register for specific setting */
+#define S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6               0xf574
+#define S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6               0xf578
+#define S5P_FIMV_D_RET_PICTURE_TIME_TOP_V6              0xf57c
+#define S5P_FIMV_D_RET_PICTURE_TIME_BOT_V6              0xf580
+#define S5P_FIMV_D_CHROMA_FORMAT_V6                     0xf588
+#define S5P_FIMV_D_MPEG4_INFO_V6                        0xf58c
+#define S5P_FIMV_D_H264_INFO_V6                         0xf590
+
+#define S5P_FIMV_D_METADATA_ADDR_CONCEALED_MB_V6        0xf594
+#define S5P_FIMV_D_METADATA_SIZE_CONCEALED_MB_V6        0xf598
+#define S5P_FIMV_D_METADATA_ADDR_VC1_PARAM_V6           0xf59c
+#define S5P_FIMV_D_METADATA_SIZE_VC1_PARAM_V6           0xf5a0
+#define S5P_FIMV_D_METADATA_ADDR_SEI_NAL_V6             0xf5a4
+#define S5P_FIMV_D_METADATA_SIZE_SEI_NAL_V6             0xf5a8
+#define S5P_FIMV_D_METADATA_ADDR_VUI_V6                 0xf5ac
+#define S5P_FIMV_D_METADATA_SIZE_VUI_V6                 0xf5b0
+
+#define S5P_FIMV_D_MVC_VIEW_ID_V6               0xf5b4
+
+/* SEI related information */
+#define S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6      0xf5f0
+#define S5P_FIMV_D_FRAME_PACK_ARRGMENT_ID_V6    0xf5f4
+#define S5P_FIMV_D_FRAME_PACK_SEI_INFO_V6       0xf5f8
+#define S5P_FIMV_D_FRAME_PACK_GRID_POS_V6       0xf5fc
+
+/* Encoder Registers */
+#define S5P_FIMV_E_FRAME_WIDTH_V6               0xf770
+#define S5P_FIMV_E_FRAME_HEIGHT_V6              0xf774
+#define S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6       0xf778
+#define S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6      0xf77c
+#define S5P_FIMV_E_FRAME_CROP_OFFSET_V6         0xf780
+#define S5P_FIMV_E_ENC_OPTIONS_V6               0xf784
+#define S5P_FIMV_E_PICTURE_PROFILE_V6           0xf788
+#define S5P_FIMV_E_FIXED_PICTURE_QP_V6          0xf790
+
+#define S5P_FIMV_E_RC_CONFIG_V6                 0xf794
+#define S5P_FIMV_E_RC_QP_BOUND_V6               0xf798
+#define S5P_FIMV_E_RC_RPARAM_V6                 0xf79c
+#define S5P_FIMV_E_MB_RC_CONFIG_V6              0xf7a0
+#define S5P_FIMV_E_PADDING_CTRL_V6              0xf7a4
+#define S5P_FIMV_E_MV_HOR_RANGE_V6              0xf7ac
+#define S5P_FIMV_E_MV_VER_RANGE_V6              0xf7b0
+
+#define S5P_FIMV_E_VBV_BUFFER_SIZE_V6           0xf84c
+#define S5P_FIMV_E_VBV_INIT_DELAY_V6            0xf850
+#define S5P_FIMV_E_NUM_DPB_V6                   0xf890
+#define S5P_FIMV_E_LUMA_DPB_V6                  0xf8c0
+#define S5P_FIMV_E_CHROMA_DPB_V6                0xf904
+#define S5P_FIMV_E_ME_BUFFER_V6                 0xf948
+
+#define S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6       0xf98c
+#define S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6       0xf990
+#define S5P_FIMV_E_TMV_BUFFER0_V6               0xf994
+#define S5P_FIMV_E_TMV_BUFFER1_V6               0xf998
+#define S5P_FIMV_E_SOURCE_LUMA_ADDR_V6          0xf9f0
+#define S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6        0xf9f4
+#define S5P_FIMV_E_STREAM_BUFFER_ADDR_V6        0xf9f8
+#define S5P_FIMV_E_STREAM_BUFFER_SIZE_V6        0xf9fc
+#define S5P_FIMV_E_ROI_BUFFER_ADDR_V6           0xfA00
+
+#define S5P_FIMV_E_PARAM_CHANGE_V6              0xfa04
+#define S5P_FIMV_E_IR_SIZE_V6                   0xfa08
+#define S5P_FIMV_E_GOP_CONFIG_V6                0xfa0c
+#define S5P_FIMV_E_MSLICE_MODE_V6               0xfa10
+#define S5P_FIMV_E_MSLICE_SIZE_MB_V6            0xfa14
+#define S5P_FIMV_E_MSLICE_SIZE_BITS_V6          0xfa18
+#define S5P_FIMV_E_FRAME_INSERTION_V6           0xfa1c
+
+#define S5P_FIMV_E_RC_FRAME_RATE_V6             0xfa20
+#define S5P_FIMV_E_RC_BIT_RATE_V6               0xfa24
+#define S5P_FIMV_E_RC_QP_OFFSET_V6              0xfa28
+#define S5P_FIMV_E_RC_ROI_CTRL_V6               0xfa2c
+#define S5P_FIMV_E_PICTURE_TAG_V6               0xfa30
+#define S5P_FIMV_E_BIT_COUNT_ENABLE_V6          0xfa34
+#define S5P_FIMV_E_MAX_BIT_COUNT_V6             0xfa38
+#define S5P_FIMV_E_MIN_BIT_COUNT_V6             0xfa3c
+
+#define S5P_FIMV_E_METADATA_BUFFER_ADDR_V6              0xfa40
+#define S5P_FIMV_E_METADATA_BUFFER_SIZE_V6              0xfa44
+#define S5P_FIMV_E_STREAM_SIZE_V6                       0xfa80
+#define S5P_FIMV_E_SLICE_TYPE_V6                        0xfa84
+#define S5P_FIMV_E_PICTURE_COUNT_V6                     0xfa88
+#define S5P_FIMV_E_RET_PICTURE_TAG_V6                   0xfa8c
+#define S5P_FIMV_E_STREAM_BUFFER_WRITE_POINTER_V6       0xfa90
+
+#define S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6          0xfa94
+#define S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6        0xfa98
+#define S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6               0xfa9c
+#define S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6             0xfaa0
+#define S5P_FIMV_E_METADATA_ADDR_ENC_SLICE_V6           0xfaa4
+#define S5P_FIMV_E_METADATA_SIZE_ENC_SLICE_V6           0xfaa8
+
+#define S5P_FIMV_E_MPEG4_OPTIONS_V6             0xfb10
+#define S5P_FIMV_E_MPEG4_HEC_PERIOD_V6          0xfb14
+#define S5P_FIMV_E_ASPECT_RATIO_V6              0xfb50
+#define S5P_FIMV_E_EXTENDED_SAR_V6              0xfb54
+
+#define S5P_FIMV_E_H264_OPTIONS_V6              0xfb58
+#define S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6      0xfb5c
+#define S5P_FIMV_E_H264_LF_BETA_OFFSET_V6       0xfb60
+#define S5P_FIMV_E_H264_I_PERIOD_V6             0xfb64
+
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6               0xfb68
+#define S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6             0xfb6c
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6             0xfb70
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6     0xfb74
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6              0xfb78
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_1_V6              0xfb7c
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_2_V6              0xfb80
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_3_V6              0xfb84
+
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6    0xfb88
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_1_V6    0xfb8c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_2_V6    0xfb90
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_3_V6    0xfb94
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_4_V6    0xfb98
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_5_V6    0xfb9c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_6_V6    0xfba0
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_7_V6    0xfba4
+
+#define S5P_FIMV_E_H264_CHROMA_QP_OFFSET_V6     0xfba8
+#define S5P_FIMV_E_H264_NUM_T_LAYER_V6          0xfbac
+
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6       0xfbb0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER1_V6       0xfbb4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER2_V6       0xfbb8
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER3_V6       0xfbbc
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER4_V6       0xfbc0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER5_V6       0xfbc4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER6_V6       0xfbc8
+
+#define S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6               0xfc4c
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_SIDE_BY_SIDE_V6        0
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TOP_BOTTOM_V6          1
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TEMPORAL_V6            2
+
+#define S5P_FIMV_E_MVC_FRAME_QP_VIEW1_V6                0xfd40
+#define S5P_FIMV_E_MVC_RC_FRAME_RATE_VIEW1_V6           0xfd44
+#define S5P_FIMV_E_MVC_RC_BIT_RATE_VIEW1_V6             0xfd48
+#define S5P_FIMV_E_MVC_RC_QBOUND_VIEW1_V6               0xfd4c
+#define S5P_FIMV_E_MVC_RC_RPARA_VIEW1_V6                0xfd50
+#define S5P_FIMV_E_MVC_INTER_VIEW_PREDICTION_ON_V6      0xfd80
+
+/* Codec numbers  */
+#define S5P_FIMV_CODEC_NONE_V6          -1
+
+
+#define S5P_FIMV_CODEC_H264_DEC_V6      0
+#define S5P_FIMV_CODEC_H264_MVC_DEC_V6  1
+
+#define S5P_FIMV_CODEC_MPEG4_DEC_V6     3
+#define S5P_FIMV_CODEC_FIMV1_DEC_V6     4
+#define S5P_FIMV_CODEC_FIMV2_DEC_V6     5
+#define S5P_FIMV_CODEC_FIMV3_DEC_V6     6
+#define S5P_FIMV_CODEC_FIMV4_DEC_V6     7
+#define S5P_FIMV_CODEC_H263_DEC_V6      8
+#define S5P_FIMV_CODEC_VC1RCV_DEC_V6    9
+#define S5P_FIMV_CODEC_VC1_DEC_V6       10
+/* FIXME: Add 11~12 */
+#define S5P_FIMV_CODEC_MPEG2_DEC_V6     13
+#define S5P_FIMV_CODEC_VP8_DEC_V6       14
+/* FIXME: Add 15~16 */
+#define S5P_FIMV_CODEC_H264_ENC_V6      20
+#define S5P_FIMV_CODEC_H264_MVC_ENC_V6  21
+
+#define S5P_FIMV_CODEC_MPEG4_ENC_V6     23
+#define S5P_FIMV_CODEC_H263_ENC_V6      24
+
+#define S5P_FIMV_NV12M_HALIGN_V6                16
+#define S5P_FIMV_NV12MT_HALIGN_V6               16
+#define S5P_FIMV_NV12MT_VALIGN_V6               16
+
+#define S5P_FIMV_TMV_BUFFER_ALIGN_V6            16
+#define S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6       256
+#define S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6     256
+#define S5P_FIMV_ME_BUFFER_ALIGN_V6             256
+#define S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6        256
+
+#define S5P_FIMV_LUMA_MB_TO_PIXEL_V6            256
+#define S5P_FIMV_CHROMA_MB_TO_PIXEL_V6          128
+#define S5P_FIMV_NUM_TMV_BUFFERS_V6             2
+
+#define S5P_FIMV_MAX_FRAME_SIZE_V6              (2 * SZ_1M)
+#define S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6        16
+#define S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6        16
+
+/* Buffer size requirements defined by hardware */
+#define S5P_FIMV_TMV_BUFFER_SIZE_V6(w, h)       (((w) + 1) * ((h) + 1) * 8)
+#define S5P_FIMV_ME_BUFFER_SIZE_V6(imw, imh, mbw, mbh) \
+        ((DIV_ROUND_UP(imw, 64) *  DIV_ROUND_UP(imh, 64) * 256) + \
+         (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(w, h)     (((w) * 192) + 64)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(w, h) \
+                        ((w) * ((h) * 64 + 144) + (2048/16 * (h) * 64) + \
+                         (2048/16 * 256 + 8320))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(w, h) \
+                                                (2096 * ((w) + (h) + 1))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(w, h)     ((w) * 400)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(w, h) \
+                        ((w) * 32 + (h) * 128 + (((w) + 1) / 2) * 64 + 2112)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(w, h) \
+                        (((w) * 64) + (((w) + 1) * 16) + (4096 * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(w, h) \
+                        (((w) * 16) + (((w) + 1) * 16))
+
+/* MFC Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V6             (28 * SZ_1K)    /*  28KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V6    (2 * SZ_1M)     /*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V6   (20 * SZ_1K)    /*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V6    (100 * SZ_1K)   /* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V6   (12 * SZ_1K)    /*  12KB */
+
+/* MFCv6 variant defines */
+#define MAX_FW_SIZE_V6                  (SZ_1M)         /* 1MB */
+#define MAX_CPB_SIZE_V6                 (3 * SZ_1M)     /* 3MB */
+#define MFC_VERSION_V6                  0x61
+#define MFC_NUM_PORTS_V6                1
+
+#endif /* _REGS_FIMV_V6_H */
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc.h b/drivers/media/platform/s5p-mfc/regs-mfc.h
index a19bece41ba9..9319e93599ae 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc.h
@@ -12,6 +12,9 @@
 #ifndef _REGS_FIMV_H
 #define _REGS_FIMV_H
 
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
 #define S5P_FIMV_REG_SIZE       (S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
 #define S5P_FIMV_REG_COUNT      ((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
 
@@ -144,6 +147,7 @@
 #define S5P_FIMV_ENC_PROFILE_H264_MAIN                  0
 #define S5P_FIMV_ENC_PROFILE_H264_HIGH                  1
 #define S5P_FIMV_ENC_PROFILE_H264_BASELINE              2
+#define S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE  3
 #define S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE               0
 #define S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE      1
 #define S5P_FIMV_ENC_PIC_STRUCT         0x083c /* picture field/frame flag */
@@ -213,6 +217,7 @@
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_MASK             (3<<4)
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_INC              (1<<4)
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_DEC              (2<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT            4
 
 /* Decode frame address */
 #define S5P_FIMV_DECODE_Y_ADR                   0x2024
@@ -377,6 +382,16 @@
 #define S5P_FIMV_R2H_CMD_EDFU_INIT_RET          16
 #define S5P_FIMV_R2H_CMD_ERR_RET                32
 
+/* Dummy definition for MFCv6 compatibilty */
+#define S5P_FIMV_CODEC_H264_MVC_DEC             -1
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET         -1
+#define S5P_FIMV_MFC_RESET                      -1
+#define S5P_FIMV_RISC_ON                        -1
+#define S5P_FIMV_RISC_BASE_ADDRESS              -1
+#define S5P_FIMV_CODEC_VP8_DEC                  -1
+#define S5P_FIMV_REG_CLEAR_BEGIN                0
+#define S5P_FIMV_REG_CLEAR_COUNT                0
+
 /* Error handling defines */
 #define S5P_FIMV_ERR_WARNINGS_START             145
 #define S5P_FIMV_ERR_DEC_MASK                   0xFFFF
@@ -414,5 +429,31 @@
 #define S5P_FIMV_SHARED_EXTENDED_SAR            0x0078
 #define S5P_FIMV_SHARED_H264_I_PERIOD           0x009C
 #define S5P_FIMV_SHARED_RC_CONTROL_CONFIG       0x00A0
+#define S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT   2
+
+/* Offset used by the hardware to store addresses */
+#define MFC_OFFSET_SHIFT        11
+
+#define FIRMWARE_ALIGN          (128 * SZ_1K)   /* 128KB */
+#define MFC_H264_CTX_BUF_SIZE   (600 * SZ_1K)   /* 600KB per H264 instance */
+#define MFC_CTX_BUF_SIZE        (10 * SZ_1K)    /* 10KB per instance */
+#define DESC_BUF_SIZE           (128 * SZ_1K)   /* 128KB for DESC buffer */
+#define SHARED_BUF_SIZE         (8 * SZ_1K)     /* 8KB for shared buffer */
+
+#define DEF_CPB_SIZE            (256 * SZ_1K)   /* 256KB */
+#define MAX_CPB_SIZE            (4 * SZ_1M)     /* 4MB */
+#define MAX_FW_SIZE             (384 * SZ_1K)
+
+#define MFC_VERSION             0x51
+#define MFC_NUM_PORTS           2
+
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_AVAIL    0x16C
+#define S5P_FIMV_SHARED_FRAME_PACK_ARRGMENT_ID  0x170
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_INFO     0x174
+#define S5P_FIMV_SHARED_FRAME_PACK_GRID_POS     0x178
+
+/* Values for resolution change in display status */
+#define S5P_FIMV_RES_INCREASE   1
+#define S5P_FIMV_RES_DECREASE   2
 
 #endif /* _REGS_FIMV_H */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 5587ef15ca4f..130f4ac8649e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -22,15 +22,15 @@
 #include <media/v4l2-event.h>
 #include <linux/workqueue.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
+#include "s5p_mfc_common.h"
 #include "s5p_mfc_ctrl.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
+#include "s5p_mfc_cmd.h"
 #include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
 
 #define S5P_MFC_NAME            "s5p-mfc"
 #define S5P_MFC_DEC_NAME        "s5p-mfc-dec"
@@ -149,10 +149,12 @@ static void s5p_mfc_watchdog_worker(struct work_struct *work)
                 if (!ctx)
                         continue;
                 ctx->state = MFCINST_ERROR;
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 clear_work_bit(ctx);
-                wake_up_ctx(ctx, S5P_FIMV_R2H_CMD_ERR_RET, 0);
+                wake_up_ctx(ctx, S5P_MFC_R2H_CMD_ERR_RET, 0);
         }
         clear_bit(0, &dev->hw_lock);
         spin_unlock_irqrestore(&dev->irqlock, flags);
@@ -199,6 +201,7 @@ static void s5p_mfc_clear_int_flags(struct s5p_mfc_dev *dev)
 static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
 {
         struct s5p_mfc_buf *dst_buf;
+        struct s5p_mfc_dev *dev = ctx->dev;
 
         ctx->state = MFCINST_FINISHED;
         ctx->sequence++;
@@ -213,8 +216,8 @@ static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
                 ctx->dst_queue_cnt--;
                 dst_buf->b->v4l2_buf.sequence = (ctx->sequence++);
 
-                if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
-                        s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+                if (s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_top, ctx) ==
+                        s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_bot, ctx))
                         dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
                 else
                         dst_buf->b->v4l2_buf.field = V4L2_FIELD_INTERLACED;
@@ -228,8 +231,11 @@ static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
 {
         struct s5p_mfc_dev *dev = ctx->dev;
         struct s5p_mfc_buf  *dst_buf, *src_buf;
-        size_t dec_y_addr = s5p_mfc_get_dec_y_adr();
-        unsigned int frame_type = s5p_mfc_get_frame_type();
+        size_t dec_y_addr;
+        unsigned int frame_type;
+
+        dec_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dec_y_adr, dev);
+        frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
 
         /* Copy timestamp / timecode from decoded src to dst and set
            appropraite flags */
@@ -265,10 +271,13 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
 {
         struct s5p_mfc_dev *dev = ctx->dev;
         struct s5p_mfc_buf  *dst_buf;
-        size_t dspl_y_addr = s5p_mfc_get_dspl_y_adr();
-        unsigned int frame_type = s5p_mfc_get_frame_type();
+        size_t dspl_y_addr;
+        unsigned int frame_type;
         unsigned int index;
 
+        dspl_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_y_adr, dev);
+        frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
+
         /* If frame is same as previous then skip and do not dequeue */
         if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
                 if (!ctx->after_packed_pb)
@@ -285,8 +294,10 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
                         list_del(&dst_buf->list);
                         ctx->dst_queue_cnt--;
                         dst_buf->b->v4l2_buf.sequence = ctx->sequence;
-                        if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
-                                s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+                        if (s5p_mfc_hw_call(dev->mfc_ops,
+                                        get_pic_type_top, ctx) ==
+                                s5p_mfc_hw_call(dev->mfc_ops,
+                                        get_pic_type_bot, ctx))
                                 dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
                         else
                                 dst_buf->b->v4l2_buf.field =
@@ -317,21 +328,23 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 
         unsigned int index;
 
-        dst_frame_status = s5p_mfc_get_dspl_status()
+        dst_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
                                 & S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
-        res_change = s5p_mfc_get_dspl_status()
-                                & S5P_FIMV_DEC_STATUS_RESOLUTION_MASK;
+        res_change = (s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
+                                & S5P_FIMV_DEC_STATUS_RESOLUTION_MASK)
+                                >> S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT;
         mfc_debug(2, "Frame Status: %x\n", dst_frame_status);
         if (ctx->state == MFCINST_RES_CHANGE_INIT)
                 ctx->state = MFCINST_RES_CHANGE_FLUSH;
-        if (res_change) {
+        if (res_change == S5P_FIMV_RES_INCREASE ||
+                res_change == S5P_FIMV_RES_DECREASE) {
                 ctx->state = MFCINST_RES_CHANGE_INIT;
-                s5p_mfc_clear_int_flags(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
                 wake_up_ctx(ctx, reason, err);
                 if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                         BUG();
                 s5p_mfc_clock_off();
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 return;
         }
         if (ctx->dpb_flush_flag)
@@ -365,9 +378,12 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
                 && !list_empty(&ctx->src_queue)) {
                 src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
                                                                 list);
-                ctx->consumed_stream += s5p_mfc_get_consumed_stream();
-                if (ctx->codec_mode != S5P_FIMV_CODEC_H264_DEC &&
-                        s5p_mfc_get_frame_type() == S5P_FIMV_DECODE_FRAME_P_FRAME
+                ctx->consumed_stream += s5p_mfc_hw_call(dev->mfc_ops,
+                                                get_consumed_stream, dev);
+                if (ctx->codec_mode != S5P_MFC_CODEC_H264_DEC &&
+                        s5p_mfc_hw_call(dev->mfc_ops,
+                                get_dec_frame_type, dev) ==
+                                        S5P_FIMV_DECODE_FRAME_P_FRAME
                                         && ctx->consumed_stream + STUFF_BYTE <
                                         src_buf->b->v4l2_planes[0].bytesused) {
                         /* Run MFC again on the same buffer */
@@ -379,7 +395,7 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
                         ctx->consumed_stream = 0;
                         list_del(&src_buf->list);
                         ctx->src_queue_cnt--;
-                        if (s5p_mfc_err_dec(err) > 0)
+                        if (s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) > 0)
                                 vb2_buffer_done(src_buf->b, VB2_BUF_STATE_ERROR);
                         else
                                 vb2_buffer_done(src_buf->b, VB2_BUF_STATE_DONE);
@@ -390,12 +406,12 @@ leave_handle_frame:
         if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
                                     || ctx->dst_queue_cnt < ctx->dpb_count)
                 clear_work_bit(ctx);
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         wake_up_ctx(ctx, reason, err);
         if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                 BUG();
         s5p_mfc_clock_off();
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Error handling for interrupt */
@@ -412,7 +428,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
 
         dev = ctx->dev;
         mfc_err("Interrupt Error: %08x\n", err);
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         wake_up_dev(dev, reason, err);
 
         /* Error recovery is dependent on the state of context */
@@ -441,9 +457,11 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
                 ctx->state = MFCINST_ERROR;
                 /* Mark all dst buffers as having an error */
                 spin_lock_irqsave(&dev->irqlock, flags);
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
                 /* Mark all src buffers as having an error */
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 spin_unlock_irqrestore(&dev->irqlock, flags);
                 if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                         BUG();
@@ -461,7 +479,6 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
                                  unsigned int reason, unsigned int err)
 {
         struct s5p_mfc_dev *dev;
-        unsigned int guard_width, guard_height;
 
         if (ctx == NULL)
                 return;
@@ -470,55 +487,44 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
                 if (ctx->c_ops->post_seq_start(ctx))
                         mfc_err("post_seq_start() failed\n");
         } else {
-                ctx->img_width = s5p_mfc_get_img_width();
-                ctx->img_height = s5p_mfc_get_img_height();
-
-                ctx->buf_width = ALIGN(ctx->img_width,
-                                                S5P_FIMV_NV12MT_HALIGN);
-                ctx->buf_height = ALIGN(ctx->img_height,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                mfc_debug(2, "SEQ Done: Movie dimensions %dx%d, "
-                        "buffer dimensions: %dx%d\n", ctx->img_width,
-                                ctx->img_height, ctx->buf_width,
-                                                ctx->buf_height);
-                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
-                        ctx->luma_size = ALIGN(ctx->buf_width *
-                                ctx->buf_height, S5P_FIMV_DEC_BUF_ALIGN);
-                        ctx->chroma_size = ALIGN(ctx->buf_width *
-                                         ALIGN((ctx->img_height >> 1),
-                                               S5P_FIMV_NV12MT_VALIGN),
-                                               S5P_FIMV_DEC_BUF_ALIGN);
-                        ctx->mv_size = ALIGN(ctx->buf_width *
-                                        ALIGN((ctx->buf_height >> 2),
-                                        S5P_FIMV_NV12MT_VALIGN),
-                                        S5P_FIMV_DEC_BUF_ALIGN);
-                } else {
-                        guard_width = ALIGN(ctx->img_width + 24,
-                                        S5P_FIMV_NV12MT_HALIGN);
-                        guard_height = ALIGN(ctx->img_height + 16,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                        ctx->luma_size = ALIGN(guard_width *
-                                guard_height, S5P_FIMV_DEC_BUF_ALIGN);
-                        guard_width = ALIGN(ctx->img_width + 16,
-                                                S5P_FIMV_NV12MT_HALIGN);
-                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                        ctx->chroma_size = ALIGN(guard_width *
-                                guard_height, S5P_FIMV_DEC_BUF_ALIGN);
-                        ctx->mv_size = 0;
-                }
-                ctx->dpb_count = s5p_mfc_get_dpb_count();
+                ctx->img_width = s5p_mfc_hw_call(dev->mfc_ops, get_img_width,
+                                dev);
+                ctx->img_height = s5p_mfc_hw_call(dev->mfc_ops, get_img_height,
+                                dev);
+
+                s5p_mfc_hw_call(dev->mfc_ops, dec_calc_dpb_size, ctx);
+
+                ctx->dpb_count = s5p_mfc_hw_call(dev->mfc_ops, get_dpb_count,
+                                dev);
+                ctx->mv_count = s5p_mfc_hw_call(dev->mfc_ops, get_mv_count,
+                                dev);
                 if (ctx->img_width == 0 || ctx->img_height == 0)
                         ctx->state = MFCINST_ERROR;
                 else
                         ctx->state = MFCINST_HEAD_PARSED;
+
+                if ((ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) &&
+                                !list_empty(&ctx->src_queue)) {
+                        struct s5p_mfc_buf *src_buf;
+                        src_buf = list_entry(ctx->src_queue.next,
+                                        struct s5p_mfc_buf, list);
+                        if (s5p_mfc_hw_call(dev->mfc_ops, get_consumed_stream,
+                                                dev) <
+                                        src_buf->b->v4l2_planes[0].bytesused)
+                                ctx->head_processed = 0;
+                        else
+                                ctx->head_processed = 1;
+                } else {
+                        ctx->head_processed = 1;
+                }
         }
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         clear_work_bit(ctx);
         if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                 BUG();
         s5p_mfc_clock_off();
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         wake_up_ctx(ctx, reason, err);
 }
 
@@ -533,14 +539,14 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
         if (ctx == NULL)
                 return;
         dev = ctx->dev;
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         ctx->int_type = reason;
         ctx->int_err = err;
         ctx->int_cond = 1;
         clear_work_bit(ctx);
         if (err == 0) {
                 ctx->state = MFCINST_RUNNING;
-                if (!ctx->dpb_flush_flag) {
+                if (!ctx->dpb_flush_flag && ctx->head_processed) {
                         spin_lock_irqsave(&dev->irqlock, flags);
                         if (!list_empty(&ctx->src_queue)) {
                                 src_buf = list_entry(ctx->src_queue.next,
@@ -560,7 +566,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
                 s5p_mfc_clock_off();
 
                 wake_up(&ctx->queue);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         } else {
                 if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                         BUG();
@@ -602,7 +608,7 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx,
 
         s5p_mfc_clock_off();
         wake_up(&ctx->queue);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Interrupt processing */
@@ -618,81 +624,83 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
         atomic_set(&dev->watchdog_cnt, 0);
         ctx = dev->ctx[dev->curr_ctx];
         /* Get the reason of interrupt and the error code */
-        reason = s5p_mfc_get_int_reason();
-        err = s5p_mfc_get_int_err();
+        reason = s5p_mfc_hw_call(dev->mfc_ops, get_int_reason, dev);
+        err = s5p_mfc_hw_call(dev->mfc_ops, get_int_err, dev);
         mfc_debug(1, "Int reason: %d (err: %08x)\n", reason, err);
         switch (reason) {
-        case S5P_FIMV_R2H_CMD_ERR_RET:
+        case S5P_MFC_R2H_CMD_ERR_RET:
                 /* An error has occured */
                 if (ctx->state == MFCINST_RUNNING &&
-                        s5p_mfc_err_dec(err) >= S5P_FIMV_ERR_WARNINGS_START)
+                        s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) >=
+                                dev->warn_start)
                         s5p_mfc_handle_frame(ctx, reason, err);
                 else
                         s5p_mfc_handle_error(ctx, reason, err);
                 clear_bit(0, &dev->enter_suspend);
                 break;
 
-        case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
-        case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+        case S5P_MFC_R2H_CMD_SLICE_DONE_RET:
+        case S5P_MFC_R2H_CMD_FIELD_DONE_RET:
+        case S5P_MFC_R2H_CMD_FRAME_DONE_RET:
                 if (ctx->c_ops->post_frame_start) {
                         if (ctx->c_ops->post_frame_start(ctx))
                                 mfc_err("post_frame_start() failed\n");
-                        s5p_mfc_clear_int_flags(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
                         wake_up_ctx(ctx, reason, err);
                         if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                                 BUG();
                         s5p_mfc_clock_off();
-                        s5p_mfc_try_run(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 } else {
                         s5p_mfc_handle_frame(ctx, reason, err);
                 }
                 break;
 
-        case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+        case S5P_MFC_R2H_CMD_SEQ_DONE_RET:
                 s5p_mfc_handle_seq_done(ctx, reason, err);
                 break;
 
-        case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
-                ctx->inst_no = s5p_mfc_get_inst_no();
+        case S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET:
+                ctx->inst_no = s5p_mfc_hw_call(dev->mfc_ops, get_inst_no, dev);
                 ctx->state = MFCINST_GOT_INST;
                 clear_work_bit(ctx);
                 wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
-        case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+        case S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET:
                 clear_work_bit(ctx);
                 ctx->state = MFCINST_FREE;
                 wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
-        case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
-        case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
-        case S5P_FIMV_R2H_CMD_SLEEP_RET:
-        case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+        case S5P_MFC_R2H_CMD_SYS_INIT_RET:
+        case S5P_MFC_R2H_CMD_FW_STATUS_RET:
+        case S5P_MFC_R2H_CMD_SLEEP_RET:
+        case S5P_MFC_R2H_CMD_WAKEUP_RET:
                 if (ctx)
                         clear_work_bit(ctx);
-                s5p_mfc_clear_int_flags(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
                 wake_up_dev(dev, reason, err);
                 clear_bit(0, &dev->hw_lock);
                 clear_bit(0, &dev->enter_suspend);
                 break;
 
-        case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+        case S5P_MFC_R2H_CMD_INIT_BUFFERS_RET:
                 s5p_mfc_handle_init_buffers(ctx, reason, err);
                 break;
 
-        case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+        case S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET:
                 s5p_mfc_handle_stream_complete(ctx, reason, err);
                 break;
 
         default:
                 mfc_debug(2, "Unknown int reason\n");
-                s5p_mfc_clear_int_flags(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         }
         mfc_debug_leave();
         return IRQ_HANDLED;
 irq_cleanup_hw:
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         ctx->int_type = reason;
         ctx->int_err = err;
         ctx->int_cond = 1;
@@ -701,7 +709,7 @@ irq_cleanup_hw:
 
         s5p_mfc_clock_off();
 
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         mfc_debug(2, "Exit via irq_cleanup_hw\n");
         return IRQ_HANDLED;
 }
@@ -749,6 +757,7 @@ static int s5p_mfc_open(struct file *file)
         if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
                 ctx->type = MFCINST_DECODER;
                 ctx->c_ops = get_dec_codec_ops();
+                s5p_mfc_dec_init(ctx);
                 /* Setup ctrl handler */
                 ret = s5p_mfc_dec_ctrls_setup(ctx);
                 if (ret) {
@@ -761,6 +770,7 @@ static int s5p_mfc_open(struct file *file)
                 /* only for encoder */
                 INIT_LIST_HEAD(&ctx->ref_queue);
                 ctx->ref_queue_cnt = 0;
+                s5p_mfc_enc_init(ctx);
                 /* Setup ctrl handler */
                 ret = s5p_mfc_enc_ctrls_setup(ctx);
                 if (ret) {
@@ -886,19 +896,20 @@ static int s5p_mfc_release(struct file *file)
                 ctx->state = MFCINST_RETURN_INST;
                 set_work_bit_irqsave(ctx);
                 s5p_mfc_clean_ctx_int_flags(ctx);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 /* Wait until instance is returned or timeout occured */
                 if (s5p_mfc_wait_for_done_ctx
-                    (ctx, S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
+                    (ctx, S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
                         s5p_mfc_clock_off();
                         mfc_err("Err returning instance\n");
                 }
                 mfc_debug(2, "After free instance\n");
                 /* Free resources */
-                s5p_mfc_release_codec_buffers(ctx);
-                s5p_mfc_release_instance_buffer(ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
                 if (ctx->type == MFCINST_DECODER)
-                        s5p_mfc_release_dec_desc_buffer(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer,
+                                        ctx);
 
                 ctx->inst_no = MFC_NO_INSTANCE_SET;
         }
@@ -910,6 +921,7 @@ static int s5p_mfc_release(struct file *file)
                 mfc_debug(2, "Last instance - release firmware\n");
                 /* reset <-> F/W release */
                 s5p_mfc_reset(dev);
+                s5p_mfc_deinit_hw(dev);
                 s5p_mfc_release_firmware(dev);
                 del_timer_sync(&dev->watchdog_timer);
                 if (s5p_mfc_power_off() < 0)
@@ -1041,6 +1053,9 @@ static int s5p_mfc_probe(struct platform_device *pdev)
                 return -ENODEV;
         }
 
+        dev->variant = (struct s5p_mfc_variant *)
+                platform_get_device_id(pdev)->driver_data;
+
         ret = s5p_mfc_init_pm(dev);
         if (ret < 0) {
                 dev_err(&pdev->dev, "failed to get mfc clock source\n");
@@ -1076,6 +1091,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
                 ret = -ENODEV;
                 goto err_res;
         }
+
         dev->mem_dev_r = device_find_child(&dev->plat_dev->dev, "s5p-mfc-r",
                                            match_child);
         if (!dev->mem_dev_r) {
@@ -1139,6 +1155,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
         vfd->release    = video_device_release,
         vfd->lock       = &dev->mfc_mutex;
         vfd->v4l2_dev   = &dev->v4l2_dev;
+        vfd->vfl_dir    = VFL_DIR_M2M;
         snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
         dev->vfd_enc    = vfd;
         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
@@ -1160,6 +1177,10 @@ static int s5p_mfc_probe(struct platform_device *pdev)
         dev->watchdog_timer.data = (unsigned long)dev;
         dev->watchdog_timer.function = s5p_mfc_watchdog;
 
+        /* Initialize HW ops and commands based on MFC version */
+        s5p_mfc_init_hw_ops(dev);
+        s5p_mfc_init_hw_cmds(dev);
+
         pr_debug("%s--\n", __func__);
         return 0;
 
@@ -1280,9 +1301,78 @@ static const struct dev_pm_ops s5p_mfc_pm_ops = {
                            NULL)
 };
 
+struct s5p_mfc_buf_size_v5 mfc_buf_size_v5 = {
+        .h264_ctx       = MFC_H264_CTX_BUF_SIZE,
+        .non_h264_ctx   = MFC_CTX_BUF_SIZE,
+        .dsc            = DESC_BUF_SIZE,
+        .shm            = SHARED_BUF_SIZE,
+};
+
+struct s5p_mfc_buf_size buf_size_v5 = {
+        .fw     = MAX_FW_SIZE,
+        .cpb    = MAX_CPB_SIZE,
+        .priv   = &mfc_buf_size_v5,
+};
+
+struct s5p_mfc_buf_align mfc_buf_align_v5 = {
+        .base = MFC_BASE_ALIGN_ORDER,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v5 = {
+        .version        = MFC_VERSION,
+        .port_num       = MFC_NUM_PORTS,
+        .buf_size       = &buf_size_v5,
+        .buf_align      = &mfc_buf_align_v5,
+        .mclk_name      = "sclk_mfc",
+        .fw_name        = "s5p-mfc.fw",
+};
+
+struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
+        .dev_ctx        = MFC_CTX_BUF_SIZE_V6,
+        .h264_dec_ctx   = MFC_H264_DEC_CTX_BUF_SIZE_V6,
+        .other_dec_ctx  = MFC_OTHER_DEC_CTX_BUF_SIZE_V6,
+        .h264_enc_ctx   = MFC_H264_ENC_CTX_BUF_SIZE_V6,
+        .other_enc_ctx  = MFC_OTHER_ENC_CTX_BUF_SIZE_V6,
+};
+
+struct s5p_mfc_buf_size buf_size_v6 = {
+        .fw     = MAX_FW_SIZE_V6,
+        .cpb    = MAX_CPB_SIZE_V6,
+        .priv   = &mfc_buf_size_v6,
+};
+
+struct s5p_mfc_buf_align mfc_buf_align_v6 = {
+        .base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v6 = {
+        .version        = MFC_VERSION_V6,
+        .port_num       = MFC_NUM_PORTS_V6,
+        .buf_size       = &buf_size_v6,
+        .buf_align      = &mfc_buf_align_v6,
+        .mclk_name      = "aclk_333",
+        .fw_name        = "s5p-mfc-v6.fw",
+};
+
+static struct platform_device_id mfc_driver_ids[] = {
+        {
+                .name = "s5p-mfc",
+                .driver_data = (unsigned long)&mfc_drvdata_v5,
+        }, {
+                .name = "s5p-mfc-v5",
+                .driver_data = (unsigned long)&mfc_drvdata_v5,
+        }, {
+                .name = "s5p-mfc-v6",
+                .driver_data = (unsigned long)&mfc_drvdata_v6,
+        },
+        {},
+};
+MODULE_DEVICE_TABLE(platform, mfc_driver_ids);
+
 static struct platform_driver s5p_mfc_driver = {
-        .probe  = s5p_mfc_probe,
-        .remove = __devexit_p(s5p_mfc_remove),
+        .probe          = s5p_mfc_probe,
+        .remove         = __devexit_p(s5p_mfc_remove),
+        .id_table       = mfc_driver_ids,
         .driver = {
                 .name   = S5P_MFC_NAME,
                 .owner  = THIS_MODULE,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
index 91a415573bd2..f0a41c95df84 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
@@ -1,7 +1,7 @@
 /*
  * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
  *
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
  *              http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -10,111 +10,20 @@
  * (at your option) any later version.
  */
 
-#include "regs-mfc.h"
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
+#include "s5p_mfc_cmd_v5.h"
+#include "s5p_mfc_cmd_v6.h"
 
-/* This function is used to send a command to the MFC */
-static int s5p_mfc_cmd_host2risc(struct s5p_mfc_dev *dev, int cmd,
-                                                struct s5p_mfc_cmd_args *args)
-{
-        int cur_cmd;
-        unsigned long timeout;
-
-        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
-        /* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
-        do {
-                if (time_after(jiffies, timeout)) {
-                        mfc_err("Timeout while waiting for hardware\n");
-                        return -EIO;
-                }
-                cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
-        } while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
-        mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
-        mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
-        mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
-        mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
-        /* Issue the command */
-        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
-        return 0;
-}
-
-/* Initialize the MFC */
-int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev)
-{
-        struct s5p_mfc_cmd_args h2r_args;
-
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = dev->fw_size;
-        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SYS_INIT, &h2r_args);
-}
-
-/* Suspend the MFC hardware */
-int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev)
-{
-        struct s5p_mfc_cmd_args h2r_args;
-
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
-}
+static struct s5p_mfc_hw_cmds *s5p_mfc_cmds;
 
-/* Wake up the MFC hardware */
-int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev)
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev)
 {
-        struct s5p_mfc_cmd_args h2r_args;
+        if (IS_MFCV6(dev))
+                s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v6();
+        else
+                s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v5();
 
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_WAKEUP, &h2r_args);
+        dev->mfc_cmds = s5p_mfc_cmds;
 }
-
-
-int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_cmd_args h2r_args;
-        int ret;
-
-        /* Preparing decoding - getting instance number */
-        mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
-        dev->curr_ctx = ctx->num;
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = ctx->codec_mode;
-        h2r_args.arg[1] = 0; /* no crc & no pixelcache */
-        h2r_args.arg[2] = ctx->ctx_ofs;
-        h2r_args.arg[3] = ctx->ctx_size;
-        ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
-                                                                &h2r_args);
-        if (ret) {
-                mfc_err("Failed to create a new instance\n");
-                ctx->state = MFCINST_ERROR;
-        }
-        return ret;
-}
-
-int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_cmd_args h2r_args;
-        int ret;
-
-        if (ctx->state == MFCINST_FREE) {
-                mfc_err("Instance already returned\n");
-                ctx->state = MFCINST_ERROR;
-                return -EINVAL;
-        }
-        /* Closing decoding instance  */
-        mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
-        dev->curr_ctx = ctx->num;
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = ctx->inst_no;
-        ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
-                                                                &h2r_args);
-        if (ret) {
-                mfc_err("Failed to return an instance\n");
-                ctx->state = MFCINST_ERROR;
-                return -EINVAL;
-        }
-        return 0;
-}
-
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
index 8b090d3723e7..282e6c780702 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
@@ -1,7 +1,7 @@
 /*
  * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
  *
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
  *              http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -21,10 +21,15 @@ struct s5p_mfc_cmd_args {
         unsigned int    arg[MAX_H2R_ARG];
 };
 
-int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx);
+struct s5p_mfc_hw_cmds {
+        int (*cmd_host2risc)(struct s5p_mfc_dev *dev, int cmd,
+                                struct s5p_mfc_cmd_args *args);
+        int (*sys_init_cmd)(struct s5p_mfc_dev *dev);
+        int (*sleep_cmd)(struct s5p_mfc_dev *dev);
+        int (*wakeup_cmd)(struct s5p_mfc_dev *dev);
+        int (*open_inst_cmd)(struct s5p_mfc_ctx *ctx);
+        int (*close_inst_cmd)(struct s5p_mfc_ctx *ctx);
+};
 
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev);
 #endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
new file mode 100644
index 000000000000..138778083c63
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -0,0 +1,166 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+
+/* This function is used to send a command to the MFC */
+int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
+                                struct s5p_mfc_cmd_args *args)
+{
+        int cur_cmd;
+        unsigned long timeout;
+
+        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+        /* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
+        do {
+                if (time_after(jiffies, timeout)) {
+                        mfc_err("Timeout while waiting for hardware\n");
+                        return -EIO;
+                }
+                cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
+        } while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
+        mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
+        mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
+        mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
+        mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
+        /* Issue the command */
+        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
+        return 0;
+}
+
+/* Initialize the MFC */
+int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        h2r_args.arg[0] = dev->fw_size;
+        return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SYS_INIT,
+                        &h2r_args);
+}
+
+/* Suspend the MFC hardware */
+int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
+}
+
+/* Wake up the MFC hardware */
+int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_WAKEUP,
+                        &h2r_args);
+}
+
+
+int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret;
+
+        /* Preparing decoding - getting instance number */
+        mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
+        dev->curr_ctx = ctx->num;
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H264_DEC;
+                break;
+        case S5P_MFC_CODEC_VC1_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_VC1_DEC;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_DEC;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG2_DEC;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H263_DEC;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_VC1RCV_DEC;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H264_ENC;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_ENC;
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H263_ENC;
+                break;
+        default:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_NONE;
+        };
+        h2r_args.arg[1] = 0; /* no crc & no pixelcache */
+        h2r_args.arg[2] = ctx->ctx.ofs;
+        h2r_args.arg[3] = ctx->ctx.size;
+        ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
+                                                                &h2r_args);
+        if (ret) {
+                mfc_err("Failed to create a new instance\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret;
+
+        if (ctx->state == MFCINST_FREE) {
+                mfc_err("Instance already returned\n");
+                ctx->state = MFCINST_ERROR;
+                return -EINVAL;
+        }
+        /* Closing decoding instance  */
+        mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
+        dev->curr_ctx = ctx->num;
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        h2r_args.arg[0] = ctx->inst_no;
+        ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
+                                                                &h2r_args);
+        if (ret) {
+                mfc_err("Failed to return an instance\n");
+                ctx->state = MFCINST_ERROR;
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/* Initialize cmd function pointers for MFC v5 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v5 = {
+        .cmd_host2risc = s5p_mfc_cmd_host2risc_v5,
+        .sys_init_cmd = s5p_mfc_sys_init_cmd_v5,
+        .sleep_cmd = s5p_mfc_sleep_cmd_v5,
+        .wakeup_cmd = s5p_mfc_wakeup_cmd_v5,
+        .open_inst_cmd = s5p_mfc_open_inst_cmd_v5,
+        .close_inst_cmd = s5p_mfc_close_inst_cmd_v5,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void)
+{
+        return &s5p_mfc_cmds_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
new file mode 100644
index 000000000000..6928a5514c1b
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_V5_H_
+#define S5P_MFC_CMD_V5_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
new file mode 100644
index 000000000000..754bfbcb1c43
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -0,0 +1,156 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "s5p_mfc_common.h"
+
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+
+int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
+                                struct s5p_mfc_cmd_args *args)
+{
+        mfc_debug(2, "Issue the command: %d\n", cmd);
+
+        /* Reset RISC2HOST command */
+        mfc_write(dev, 0x0, S5P_FIMV_RISC2HOST_CMD_V6);
+
+        /* Issue the command */
+        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD_V6);
+        mfc_write(dev, 0x1, S5P_FIMV_HOST2RISC_INT_V6);
+
+        return 0;
+}
+
+int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+        struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+        s5p_mfc_hw_call(dev->mfc_ops, alloc_dev_context_buffer, dev);
+        mfc_write(dev, dev->ctx_buf.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+        mfc_write(dev, buf_size->dev_ctx, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SYS_INIT_V6,
+                                        &h2r_args);
+}
+
+int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SLEEP_V6,
+                        &h2r_args);
+}
+
+int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_WAKEUP_V6,
+                                        &h2r_args);
+}
+
+/* Open a new instance and get its number */
+int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int codec_type;
+
+        mfc_debug(2, "Requested codec mode: %d\n", ctx->codec_mode);
+        dev->curr_ctx = ctx->num;
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                codec_type = S5P_FIMV_CODEC_H264_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_H264_MVC_DEC:
+                codec_type = S5P_FIMV_CODEC_H264_MVC_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_VC1_DEC:
+                codec_type = S5P_FIMV_CODEC_VC1_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                codec_type = S5P_FIMV_CODEC_MPEG4_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                codec_type = S5P_FIMV_CODEC_MPEG2_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                codec_type = S5P_FIMV_CODEC_H263_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+                codec_type = S5P_FIMV_CODEC_VC1RCV_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_VP8_DEC:
+                codec_type = S5P_FIMV_CODEC_VP8_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                codec_type = S5P_FIMV_CODEC_H264_ENC_V6;
+                break;
+        case S5P_MFC_CODEC_H264_MVC_ENC:
+                codec_type = S5P_FIMV_CODEC_H264_MVC_ENC_V6;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                codec_type = S5P_FIMV_CODEC_MPEG4_ENC_V6;
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                codec_type = S5P_FIMV_CODEC_H263_ENC_V6;
+                break;
+        default:
+                codec_type = S5P_FIMV_CODEC_NONE_V6;
+        };
+        mfc_write(dev, codec_type, S5P_FIMV_CODEC_TYPE_V6);
+        mfc_write(dev, ctx->ctx.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+        mfc_write(dev, ctx->ctx.size, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+        mfc_write(dev, 0, S5P_FIMV_D_CRC_CTRL_V6); /* no crc */
+
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6,
+                                        &h2r_args);
+}
+
+/* Close instance */
+int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret = 0;
+
+        dev->curr_ctx = ctx->num;
+        if (ctx->state != MFCINST_FREE) {
+                mfc_write(dev, ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+                ret = s5p_mfc_cmd_host2risc_v6(dev,
+                                        S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6,
+                                        &h2r_args);
+        } else {
+                ret = -EINVAL;
+        }
+
+        return ret;
+}
+
+/* Initialize cmd function pointers for MFC v6 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v6 = {
+        .cmd_host2risc = s5p_mfc_cmd_host2risc_v6,
+        .sys_init_cmd = s5p_mfc_sys_init_cmd_v6,
+        .sleep_cmd = s5p_mfc_sleep_cmd_v6,
+        .wakeup_cmd = s5p_mfc_wakeup_cmd_v6,
+        .open_inst_cmd = s5p_mfc_open_inst_cmd_v6,
+        .close_inst_cmd = s5p_mfc_close_inst_cmd_v6,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void)
+{
+        return &s5p_mfc_cmds_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
new file mode 100644
index 000000000000..b7a8e57837b5
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_V6_H_
+#define S5P_MFC_CMD_V6_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 519b0d66d8d1..f02e0497ca98 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -16,13 +16,14 @@
 #ifndef S5P_MFC_COMMON_H_
 #define S5P_MFC_COMMON_H_
 
-#include "regs-mfc.h"
 #include <linux/platform_device.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-core.h>
+#include "regs-mfc.h"
+#include "regs-mfc-v6.h"
 
 /* Definitions related to MFC memory */
 
@@ -30,17 +31,6 @@
 *  while mmaping */
 #define DST_QUEUE_OFF_BASE      (TASK_SIZE / 2)
 
-/* Offset used by the hardware to store addresses */
-#define MFC_OFFSET_SHIFT        11
-
-#define FIRMWARE_ALIGN          0x20000         /* 128KB */
-#define MFC_H264_CTX_BUF_SIZE   0x96000         /* 600KB per H264 instance */
-#define MFC_CTX_BUF_SIZE        0x2800          /* 10KB per instance */
-#define DESC_BUF_SIZE           0x20000         /* 128KB for DESC buffer */
-#define SHARED_BUF_SIZE         0x2000          /* 8KB for shared buffer */
-
-#define DEF_CPB_SIZE            0x40000         /* 512KB */
-
 #define MFC_BANK1_ALLOC_CTX     0
 #define MFC_BANK2_ALLOC_CTX     1
 
@@ -74,7 +64,40 @@ static inline dma_addr_t s5p_mfc_mem_cookie(void *a, void *b)
 #define MFC_ENC_CAP_PLANE_COUNT 1
 #define MFC_ENC_OUT_PLANE_COUNT 2
 #define STUFF_BYTE              4
-#define MFC_MAX_CTRLS           64
+#define MFC_MAX_CTRLS           70
+
+#define S5P_MFC_CODEC_NONE              -1
+#define S5P_MFC_CODEC_H264_DEC          0
+#define S5P_MFC_CODEC_H264_MVC_DEC      1
+#define S5P_MFC_CODEC_VC1_DEC           2
+#define S5P_MFC_CODEC_MPEG4_DEC         3
+#define S5P_MFC_CODEC_MPEG2_DEC         4
+#define S5P_MFC_CODEC_H263_DEC          5
+#define S5P_MFC_CODEC_VC1RCV_DEC        6
+#define S5P_MFC_CODEC_VP8_DEC           7
+
+#define S5P_MFC_CODEC_H264_ENC          20
+#define S5P_MFC_CODEC_H264_MVC_ENC      21
+#define S5P_MFC_CODEC_MPEG4_ENC         22
+#define S5P_MFC_CODEC_H263_ENC          23
+
+#define S5P_MFC_R2H_CMD_EMPTY                   0
+#define S5P_MFC_R2H_CMD_SYS_INIT_RET            1
+#define S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET       2
+#define S5P_MFC_R2H_CMD_SEQ_DONE_RET            3
+#define S5P_MFC_R2H_CMD_INIT_BUFFERS_RET        4
+#define S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET      6
+#define S5P_MFC_R2H_CMD_SLEEP_RET               7
+#define S5P_MFC_R2H_CMD_WAKEUP_RET              8
+#define S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET        9
+#define S5P_MFC_R2H_CMD_DPB_FLUSH_RET           10
+#define S5P_MFC_R2H_CMD_NAL_ABORT_RET           11
+#define S5P_MFC_R2H_CMD_FW_STATUS_RET           12
+#define S5P_MFC_R2H_CMD_FRAME_DONE_RET          13
+#define S5P_MFC_R2H_CMD_FIELD_DONE_RET          14
+#define S5P_MFC_R2H_CMD_SLICE_DONE_RET          15
+#define S5P_MFC_R2H_CMD_ENC_BUFFER_FUL_RET      16
+#define S5P_MFC_R2H_CMD_ERR_RET                 32
 
 #define mfc_read(dev, offset)           readl(dev->regs_base + (offset))
 #define mfc_write(dev, data, offset)    writel((data), dev->regs_base + \
@@ -177,6 +200,58 @@ struct s5p_mfc_pm {
         struct device   *device;
 };
 
+struct s5p_mfc_buf_size_v5 {
+        unsigned int h264_ctx;
+        unsigned int non_h264_ctx;
+        unsigned int dsc;
+        unsigned int shm;
+};
+
+struct s5p_mfc_buf_size_v6 {
+        unsigned int dev_ctx;
+        unsigned int h264_dec_ctx;
+        unsigned int other_dec_ctx;
+        unsigned int h264_enc_ctx;
+        unsigned int other_enc_ctx;
+};
+
+struct s5p_mfc_buf_size {
+        unsigned int fw;
+        unsigned int cpb;
+        void *priv;
+};
+
+struct s5p_mfc_buf_align {
+        unsigned int base;
+};
+
+struct s5p_mfc_variant {
+        unsigned int version;
+        unsigned int port_num;
+        struct s5p_mfc_buf_size *buf_size;
+        struct s5p_mfc_buf_align *buf_align;
+        char    *mclk_name;
+        char    *fw_name;
+};
+
+/**
+ * struct s5p_mfc_priv_buf - represents internal used buffer
+ * @alloc:              allocation-specific context for each buffer
+ *                      (videobuf2 allocator)
+ * @ofs:                offset of each buffer, will be used for MFC
+ * @virt:               kernel virtual address, only valid when the
+ *                      buffer accessed by driver
+ * @dma:                DMA address, only valid when kernel DMA API used
+ * @size:               size of the buffer
+ */
+struct s5p_mfc_priv_buf {
+        void            *alloc;
+        unsigned long   ofs;
+        void            *virt;
+        dma_addr_t      dma;
+        size_t          size;
+};
+
 /**
  * struct s5p_mfc_dev - The struct containing driver internal parameters.
  *
@@ -191,6 +266,7 @@ struct s5p_mfc_pm {
  * @dec_ctrl_handler:   control framework handler for decoding
  * @enc_ctrl_handler:   control framework handler for encoding
  * @pm:                 power management control
+ * @variant:            MFC hardware variant information
  * @num_inst:           couter of active MFC instances
  * @irqlock:            lock for operations on videobuf2 queues
  * @condlock:           lock for changing/checking if a context is ready to be
@@ -212,6 +288,10 @@ struct s5p_mfc_pm {
  * @watchdog_work:      worker for the watchdog
  * @alloc_ctx:          videobuf2 allocator contexts for two memory banks
  * @enter_suspend:      flag set when entering suspend
+ * @ctx_buf:            common context memory (MFCv6)
+ * @warn_start:         hardware error code from which warnings start
+ * @mfc_ops:            ops structure holding HW operation function pointers
+ * @mfc_cmds:           cmd structure holding HW commands function pointers
  *
  */
 struct s5p_mfc_dev {
@@ -226,6 +306,7 @@ struct s5p_mfc_dev {
         struct v4l2_ctrl_handler dec_ctrl_handler;
         struct v4l2_ctrl_handler enc_ctrl_handler;
         struct s5p_mfc_pm       pm;
+        struct s5p_mfc_variant  *variant;
         int num_inst;
         spinlock_t irqlock;     /* lock when operating on videobuf2 queues */
         spinlock_t condlock;    /* lock when changing/checking if a context is
@@ -248,6 +329,11 @@ struct s5p_mfc_dev {
         struct work_struct watchdog_work;
         void *alloc_ctx[2];
         unsigned long enter_suspend;
+
+        struct s5p_mfc_priv_buf ctx_buf;
+        int warn_start;
+        struct s5p_mfc_hw_ops *mfc_ops;
+        struct s5p_mfc_hw_cmds *mfc_cmds;
 };
 
 /**
@@ -262,7 +348,6 @@ struct s5p_mfc_h264_enc_params {
         u8 max_ref_pic;
         u8 num_ref_pic_4p;
         int _8x8_transform;
-        int rc_mb;
         int rc_mb_dark;
         int rc_mb_smooth;
         int rc_mb_static;
@@ -281,6 +366,23 @@ struct s5p_mfc_h264_enc_params {
         enum v4l2_mpeg_video_h264_level level_v4l2;
         int level;
         u16 cpb_size;
+        int interlace;
+        u8 hier_qp;
+        u8 hier_qp_type;
+        u8 hier_qp_layer;
+        u8 hier_qp_layer_qp[7];
+        u8 sei_frame_packing;
+        u8 sei_fp_curr_frame_0;
+        u8 sei_fp_arrangement_type;
+
+        u8 fmo;
+        u8 fmo_map_type;
+        u8 fmo_slice_grp;
+        u8 fmo_chg_dir;
+        u32 fmo_chg_rate;
+        u32 fmo_run_len[4];
+        u8 aso;
+        u32 aso_slice_order[8];
 };
 
 /**
@@ -319,9 +421,11 @@ struct s5p_mfc_enc_params {
         u8 pad_cb;
         u8 pad_cr;
         int rc_frame;
+        int rc_mb;
         u32 rc_bitrate;
         u16 rc_reaction_coeff;
         u16 vbv_size;
+        u32 vbv_delay;
 
         enum v4l2_mpeg_video_header_mode seq_hdr_mode;
         enum v4l2_mpeg_mfc51_video_frame_skip_mode frame_skip_mode;
@@ -330,7 +434,6 @@ struct s5p_mfc_enc_params {
         u8 num_b_frame;
         u32 rc_framerate_num;
         u32 rc_framerate_denom;
-        int interlace;
 
         union {
                 struct s5p_mfc_h264_enc_params h264;
@@ -388,6 +491,8 @@ struct s5p_mfc_codec_ops {
  *                      decoding buffer
  * @dpb_flush_flag:     flag used to indicate that a DPB buffers are being
  *                      flushed
+ * @head_processed:     flag mentioning whether the header data is processed
+ *                      completely or not
  * @bank1_buf:          handle to memory allocated for temporary buffers from
  *                      memory bank 1
  * @bank1_phys:         address of the temporary buffers from memory bank 1
@@ -412,19 +517,20 @@ struct s5p_mfc_codec_ops {
  * @display_delay_enable:       display delay for H264 enable flag
  * @after_packed_pb:    flag used to track buffer when stream is in
  *                      Packed PB format
+ * @sei_fp_parse:       enable/disable parsing of frame packing SEI information
  * @dpb_count:          count of the DPB buffers required by MFC hw
  * @total_dpb_count:    count of DPB buffers with additional buffers
  *                      requested by the application
- * @ctx_buf:            handle to the memory associated with this context
- * @ctx_phys:           address of the memory associated with this context
- * @ctx_size:           size of the memory associated with this context
- * @desc_buf:           description buffer for decoding handle
- * @desc_phys:          description buffer for decoding address
- * @shm_alloc:          handle for the shared memory buffer
- * @shm:                virtual address for the shared memory buffer
- * @shm_ofs:            address offset for shared memory
+ * @ctx:                context buffer information
+ * @dsc:                descriptor buffer information
+ * @shm:                shared memory buffer information
+ * @mv_count:           number of MV buffers allocated for decoding
  * @enc_params:         encoding parameters for MFC
  * @enc_dst_buf_size:   size of the buffers for encoder output
+ * @luma_dpb_size:      dpb buffer size for luma
+ * @chroma_dpb_size:    dpb buffer size for chroma
+ * @me_buffer_size:     size of the motion estimation buffer
+ * @tmv_buffer_size:    size of temporal predictor motion vector buffer
  * @frame_type:         used to force the type of the next encoded frame
  * @ref_queue:          list of the reference buffers for encoding
  * @ref_queue_cnt:      number of the buffers in the reference list
@@ -473,6 +579,7 @@ struct s5p_mfc_ctx {
         unsigned long consumed_stream;
 
         unsigned int dpb_flush_flag;
+        unsigned int head_processed;
 
         /* Buffers */
         void *bank1_buf;
@@ -502,37 +609,41 @@ struct s5p_mfc_ctx {
         int display_delay;
         int display_delay_enable;
         int after_packed_pb;
+        int sei_fp_parse;
 
         int dpb_count;
         int total_dpb_count;
-
+        int mv_count;
         /* Buffers */
-        void *ctx_buf;
-        size_t ctx_phys;
-        size_t ctx_ofs;
-        size_t ctx_size;
-
-        void *desc_buf;
-        size_t desc_phys;
-
-
-        void *shm_alloc;
-        void *shm;
-        size_t shm_ofs;
+        struct s5p_mfc_priv_buf ctx;
+        struct s5p_mfc_priv_buf dsc;
+        struct s5p_mfc_priv_buf shm;
 
         struct s5p_mfc_enc_params enc_params;
 
         size_t enc_dst_buf_size;
+        size_t luma_dpb_size;
+        size_t chroma_dpb_size;
+        size_t me_buffer_size;
+        size_t tmv_buffer_size;
 
         enum v4l2_mpeg_mfc51_video_force_frame_type force_frame_type;
 
         struct list_head ref_queue;
         unsigned int ref_queue_cnt;
 
+        enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+        union {
+                unsigned int mb;
+                unsigned int bits;
+        } slice_size;
+
         struct s5p_mfc_codec_ops *c_ops;
 
         struct v4l2_ctrl *ctrls[MFC_MAX_CTRLS];
         struct v4l2_ctrl_handler ctrl_handler;
+        unsigned int frame_tag;
+        size_t scratch_buf_size;
 };
 
 /*
@@ -565,6 +676,9 @@ struct mfc_control {
         __u8                    is_volatile;
 };
 
+/* Macro for making hardware specific calls */
+#define s5p_mfc_hw_call(f, op, args...) \
+        ((f && f->op) ? f->op(args) : -ENODEV)
 
 #define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
 #define ctrl_to_ctx(__ctrl) \
@@ -575,4 +689,9 @@ void set_work_bit(struct s5p_mfc_ctx *ctx);
 void clear_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 
+#define HAS_PORTNUM(dev)        (dev ? (dev->variant ? \
+                                (dev->variant->port_num ? 1 : 0) : 0) : 0)
+#define IS_TWOPORT(dev)         (dev->variant->port_num == 2 ? 1 : 0)
+#define IS_MFCV6(dev)           (dev->variant->version >= 0x60 ? 1 : 0)
+
 #endif /* S5P_MFC_COMMON_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index 0deba6bc687c..585b7b0ed8ec 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -15,11 +15,11 @@
 #include <linux/firmware.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
 
 static void *s5p_mfc_bitproc_buf;
@@ -37,13 +37,19 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
         /* Firmare has to be present as a separate file or compiled
          * into kernel. */
         mfc_debug_enter();
+
         err = request_firmware((const struct firmware **)&fw_blob,
-                                     "s5p-mfc.fw", dev->v4l2_dev.dev);
+                                     dev->variant->fw_name, dev->v4l2_dev.dev);
         if (err != 0) {
                 mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
                 return -EINVAL;
         }
-        dev->fw_size = ALIGN(fw_blob->size, FIRMWARE_ALIGN);
+        dev->fw_size = dev->variant->buf_size->fw;
+        if (fw_blob->size > dev->fw_size) {
+                mfc_err("MFC firmware is too big to be loaded\n");
+                release_firmware(fw_blob);
+                return -ENOMEM;
+        }
         if (s5p_mfc_bitproc_buf) {
                 mfc_err("Attempting to allocate firmware when it seems that it is already loaded\n");
                 release_firmware(fw_blob);
@@ -77,32 +83,37 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
                 return -EIO;
         }
         dev->bank1 = s5p_mfc_bitproc_phys;
-        b_base = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], 1 << MFC_BASE_ALIGN_ORDER);
-        if (IS_ERR(b_base)) {
-                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-                s5p_mfc_bitproc_phys = 0;
-                s5p_mfc_bitproc_buf = NULL;
-                mfc_err("Allocating bank2 base failed\n");
-        release_firmware(fw_blob);
-                return -ENOMEM;
-        }
-        bank2_base_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
-        vb2_dma_contig_memops.put(b_base);
-        if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
-                mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
-                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-                s5p_mfc_bitproc_phys = 0;
-                s5p_mfc_bitproc_buf = NULL;
-                release_firmware(fw_blob);
-                return -EIO;
+        if (HAS_PORTNUM(dev) && IS_TWOPORT(dev)) {
+                b_base = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK2_ALLOC_CTX],
+                        1 << MFC_BASE_ALIGN_ORDER);
+                if (IS_ERR(b_base)) {
+                        vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                        s5p_mfc_bitproc_phys = 0;
+                        s5p_mfc_bitproc_buf = NULL;
+                        mfc_err("Allocating bank2 base failed\n");
+                        release_firmware(fw_blob);
+                        return -ENOMEM;
+                }
+                bank2_base_phys = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
+                vb2_dma_contig_memops.put(b_base);
+                if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
+                        mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
+                        vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                        s5p_mfc_bitproc_phys = 0;
+                        s5p_mfc_bitproc_buf = NULL;
+                        release_firmware(fw_blob);
+                        return -EIO;
+                }
+                /* Valid buffers passed to MFC encoder with LAST_FRAME command
+                 * should not have address of bank2 - MFC will treat it as a null frame.
+                 * To avoid such situation we set bank2 address below the pool address.
+                 */
+                dev->bank2 = bank2_base_phys - (1 << MFC_BASE_ALIGN_ORDER);
+        } else {
+                dev->bank2 = dev->bank1;
         }
-        /* Valid buffers passed to MFC encoder with LAST_FRAME command
-         * should not have address of bank2 - MFC will treat it as a null frame.
-         * To avoid such situation we set bank2 address below the pool address.
-         */
-        dev->bank2 = bank2_base_phys - (1 << MFC_BASE_ALIGN_ORDER);
         memcpy(s5p_mfc_bitproc_virt, fw_blob->data, fw_blob->size);
         wmb();
         release_firmware(fw_blob);
@@ -119,8 +130,9 @@ int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
         /* Firmare has to be present as a separate file or compiled
          * into kernel. */
         mfc_debug_enter();
+
         err = request_firmware((const struct firmware **)&fw_blob,
-                                     "s5p-mfc.fw", dev->v4l2_dev.dev);
+                                     dev->variant->fw_name, dev->v4l2_dev.dev);
         if (err != 0) {
                 mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
                 return -EINVAL;
@@ -161,46 +173,81 @@ int s5p_mfc_reset(struct s5p_mfc_dev *dev)
 {
         unsigned int mc_status;
         unsigned long timeout;
+        int i;
 
         mfc_debug_enter();
-        /* Stop procedure */
-        /*  reset RISC */
-        mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
-        /*  All reset except for MC */
-        mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
-        mdelay(10);
-
-        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
-        /* Check MC status */
-        do {
-                if (time_after(jiffies, timeout)) {
-                        mfc_err("Timeout while resetting MFC\n");
-                        return -EIO;
-                }
 
-                mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+        if (IS_MFCV6(dev)) {
+                /* Reset IP */
+                /*  except RISC, reset */
+                mfc_write(dev, 0xFEE, S5P_FIMV_MFC_RESET_V6);
+                /*  reset release */
+                mfc_write(dev, 0x0, S5P_FIMV_MFC_RESET_V6);
+
+                /* Zero Initialization of MFC registers */
+                mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+                mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD_V6);
+                mfc_write(dev, 0, S5P_FIMV_FW_VERSION_V6);
+
+                for (i = 0; i < S5P_FIMV_REG_CLEAR_COUNT_V6; i++)
+                        mfc_write(dev, 0, S5P_FIMV_REG_CLEAR_BEGIN_V6 + (i*4));
 
-        } while (mc_status & 0x3);
+                /* Reset */
+                mfc_write(dev, 0, S5P_FIMV_RISC_ON_V6);
+                mfc_write(dev, 0x1FFF, S5P_FIMV_MFC_RESET_V6);
+                mfc_write(dev, 0, S5P_FIMV_MFC_RESET_V6);
+        } else {
+                /* Stop procedure */
+                /*  reset RISC */
+                mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
+                /*  All reset except for MC */
+                mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
+                mdelay(10);
+
+                timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+                /* Check MC status */
+                do {
+                        if (time_after(jiffies, timeout)) {
+                                mfc_err("Timeout while resetting MFC\n");
+                                return -EIO;
+                        }
+
+                        mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+
+                } while (mc_status & 0x3);
+
+                mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
+                mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
+        }
 
-        mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
-        mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
         mfc_debug_leave();
         return 0;
 }
 
 static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
 {
-        mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
-        mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
-        mfc_debug(2, "Bank1: %08x, Bank2: %08x\n", dev->bank1, dev->bank2);
+        if (IS_MFCV6(dev)) {
+                mfc_write(dev, dev->bank1, S5P_FIMV_RISC_BASE_ADDRESS_V6);
+                mfc_debug(2, "Base Address : %08x\n", dev->bank1);
+        } else {
+                mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
+                mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
+                mfc_debug(2, "Bank1: %08x, Bank2: %08x\n",
+                                dev->bank1, dev->bank2);
+        }
 }
 
 static inline void s5p_mfc_clear_cmds(struct s5p_mfc_dev *dev)
 {
-        mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
-        mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
-        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
-        mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+        if (IS_MFCV6(dev)) {
+                /* Zero initialization should be done before RESET.
+                 * Nothing to do here. */
+        } else {
+                mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
+                mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
+                mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+                mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+        }
 }
 
 /* Initialize hardware */
@@ -228,9 +275,12 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         s5p_mfc_clear_cmds(dev);
         /* 3. Release reset signal to the RISC */
         s5p_mfc_clean_dev_int_flags(dev);
-        mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+        if (IS_MFCV6(dev))
+                mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+        else
+                mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
         mfc_debug(2, "Will now wait for completion of firmware transfer\n");
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_FW_STATUS_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_FW_STATUS_RET)) {
                 mfc_err("Failed to load firmware\n");
                 s5p_mfc_reset(dev);
                 s5p_mfc_clock_off();
@@ -238,7 +288,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         }
         s5p_mfc_clean_dev_int_flags(dev);
         /* 4. Initialize firmware */
-        ret = s5p_mfc_sys_init_cmd(dev);
+        ret = s5p_mfc_hw_call(dev->mfc_cmds, sys_init_cmd, dev);
         if (ret) {
                 mfc_err("Failed to send command to MFC - timeout\n");
                 s5p_mfc_reset(dev);
@@ -246,7 +296,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
                 return ret;
         }
         mfc_debug(2, "Ok, now will write a command to init the system\n");
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SYS_INIT_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SYS_INIT_RET)) {
                 mfc_err("Failed to load firmware\n");
                 s5p_mfc_reset(dev);
                 s5p_mfc_clock_off();
@@ -254,7 +304,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         }
         dev->int_cond = 0;
         if (dev->int_err != 0 || dev->int_type !=
-                                        S5P_FIMV_R2H_CMD_SYS_INIT_RET) {
+                                        S5P_MFC_R2H_CMD_SYS_INIT_RET) {
                 /* Failure. */
                 mfc_err("Failed to init firmware - error: %d int: %d\n",
                                                 dev->int_err, dev->int_type);
@@ -262,7 +312,11 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
                 s5p_mfc_clock_off();
                 return -EIO;
         }
-        ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+        if (IS_MFCV6(dev))
+                ver = mfc_read(dev, S5P_FIMV_FW_VERSION_V6);
+        else
+                ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+
         mfc_debug(2, "MFC F/W version : %02xyy, %02xmm, %02xdd\n",
                 (ver >> 16) & 0xFF, (ver >> 8) & 0xFF, ver & 0xFF);
         s5p_mfc_clock_off();
@@ -271,6 +325,17 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 }
 
 
+/* Deinitialize hardware */
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev)
+{
+        s5p_mfc_clock_on();
+
+        s5p_mfc_reset(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, release_dev_context_buffer, dev);
+
+        s5p_mfc_clock_off();
+}
+
 int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
 {
         int ret;
@@ -278,19 +343,19 @@ int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
         mfc_debug_enter();
         s5p_mfc_clock_on();
         s5p_mfc_clean_dev_int_flags(dev);
-        ret = s5p_mfc_sleep_cmd(dev);
+        ret = s5p_mfc_hw_call(dev->mfc_cmds, sleep_cmd, dev);
         if (ret) {
                 mfc_err("Failed to send command to MFC - timeout\n");
                 return ret;
         }
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SLEEP_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SLEEP_RET)) {
                 mfc_err("Failed to sleep\n");
                 return -EIO;
         }
         s5p_mfc_clock_off();
         dev->int_cond = 0;
         if (dev->int_err != 0 || dev->int_type !=
-                                                S5P_FIMV_R2H_CMD_SLEEP_RET) {
+                                                S5P_MFC_R2H_CMD_SLEEP_RET) {
                 /* Failure. */
                 mfc_err("Failed to sleep - error: %d int: %d\n", dev->int_err,
                                                                 dev->int_type);
@@ -320,22 +385,25 @@ int s5p_mfc_wakeup(struct s5p_mfc_dev *dev)
         s5p_mfc_clear_cmds(dev);
         s5p_mfc_clean_dev_int_flags(dev);
         /* 3. Initialize firmware */
-        ret = s5p_mfc_wakeup_cmd(dev);
+        ret = s5p_mfc_hw_call(dev->mfc_cmds, wakeup_cmd, dev);
         if (ret) {
                 mfc_err("Failed to send command to MFC - timeout\n");
                 return ret;
         }
         /* 4. Release reset signal to the RISC */
-        mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+        if (IS_MFCV6(dev))
+                mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+        else
+                mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
         mfc_debug(2, "Ok, now will write a command to wakeup the system\n");
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_WAKEUP_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_WAKEUP_RET)) {
                 mfc_err("Failed to load firmware\n");
                 return -EIO;
         }
         s5p_mfc_clock_off();
         dev->int_cond = 0;
         if (dev->int_err != 0 || dev->int_type !=
-                                                S5P_FIMV_R2H_CMD_WAKEUP_RET) {
+                                                S5P_MFC_R2H_CMD_WAKEUP_RET) {
                 /* Failure. */
                 mfc_err("Failed to wakeup - error: %d int: %d\n", dev->int_err,
                                                                 dev->int_type);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
index e1e0c544b6a2..90aa9b9886d5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
@@ -20,6 +20,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev);
 int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_init_hw(struct s5p_mfc_dev *dev);
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_sleep(struct s5p_mfc_dev *dev);
 int s5p_mfc_wakeup(struct s5p_mfc_dev *dev);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 6ee21bb71398..eb6a70b0f821 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -23,85 +23,114 @@
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
+
+#define DEF_SRC_FMT_DEC V4L2_PIX_FMT_H264
+#define DEF_DST_FMT_DEC V4L2_PIX_FMT_NV12MT_16X16
 
 static struct s5p_mfc_fmt formats[] = {
         {
+                .name           = "4:2:0 2 Planes 16x16 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT_16X16,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
                 .name           = "4:2:0 2 Planes 64x32 Tiles",
                 .fourcc         = V4L2_PIX_FMT_NV12MT,
-                .codec_mode     = S5P_FIMV_CODEC_NONE,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
                 .type           = MFC_FMT_RAW,
                 .num_planes     = 2,
-         },
+        },
         {
-                .name = "4:2:0 2 Planes",
-                .fourcc = V4L2_PIX_FMT_NV12M,
-                .codec_mode = S5P_FIMV_CODEC_NONE,
-                .type = MFC_FMT_RAW,
-                .num_planes = 2,
+                .name           = "4:2:0 2 Planes Y/CbCr",
+                .fourcc         = V4L2_PIX_FMT_NV12M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
+                .name           = "4:2:0 2 Planes Y/CrCb",
+                .fourcc         = V4L2_PIX_FMT_NV21M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
         },
         {
-                .name = "H264 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H264,
-                .codec_mode = S5P_FIMV_CODEC_H264_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "H264 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H264,
+                .codec_mode     = S5P_MFC_CODEC_H264_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "H263 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H263,
-                .codec_mode = S5P_FIMV_CODEC_H263_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "H264/MVC Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H264_MVC,
+                .codec_mode     = S5P_MFC_CODEC_H264_MVC_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG1 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG1,
-                .codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "H263 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H263,
+                .codec_mode     = S5P_MFC_CODEC_H263_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG2 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG2,
-                .codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "MPEG1 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG1,
+                .codec_mode     = S5P_MFC_CODEC_MPEG2_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG4 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG4,
-                .codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "MPEG2 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG2,
+                .codec_mode     = S5P_MFC_CODEC_MPEG2_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "XviD Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_XVID,
-                .codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "MPEG4 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG4,
+                .codec_mode     = S5P_MFC_CODEC_MPEG4_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "VC1 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_VC1_ANNEX_G,
-                .codec_mode = S5P_FIMV_CODEC_VC1_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "XviD Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_XVID,
+                .codec_mode     = S5P_MFC_CODEC_MPEG4_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "VC1 RCV Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_VC1_ANNEX_L,
-                .codec_mode = S5P_FIMV_CODEC_VC1RCV_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "VC1 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_VC1_ANNEX_G,
+                .codec_mode     = S5P_MFC_CODEC_VC1_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
+        },
+        {
+                .name           = "VC1 RCV Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_VC1_ANNEX_L,
+                .codec_mode     = S5P_MFC_CODEC_VC1RCV_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
+        },
+        {
+                .name           = "VP8 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_VP8,
+                .codec_mode     = S5P_MFC_CODEC_VP8_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
 };
 
@@ -297,7 +326,7 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 /* If the MFC is parsing the header,
                  * so wait until it is finished */
                 s5p_mfc_clean_ctx_int_flags(ctx);
-                s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_SEQ_DONE_RET,
+                s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_SEQ_DONE_RET,
                                                                         0);
         }
         if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
@@ -342,21 +371,36 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 /* Try format */
 static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
+        struct s5p_mfc_dev *dev = video_drvdata(file);
         struct s5p_mfc_fmt *fmt;
 
-        if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-                mfc_err("This node supports decoding only\n");
-                return -EINVAL;
-        }
-        fmt = find_format(f, MFC_FMT_DEC);
-        if (!fmt) {
-                mfc_err("Unsupported format\n");
-                return -EINVAL;
-        }
-        if (fmt->type != MFC_FMT_DEC) {
-                mfc_err("\n");
-                return -EINVAL;
+        mfc_debug(2, "Type is %d\n", f->type);
+        if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                fmt = find_format(f, MFC_FMT_DEC);
+                if (!fmt) {
+                        mfc_err("Unsupported format for source.\n");
+                        return -EINVAL;
+                }
+                if (!IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
+        } else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                fmt = find_format(f, MFC_FMT_RAW);
+                if (!fmt) {
+                        mfc_err("Unsupported format for destination.\n");
+                        return -EINVAL;
+                }
+                if (IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                } else if (!IS_MFCV6(dev) &&
+                                (fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
         }
+
         return 0;
 }
 
@@ -379,8 +423,29 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 ret = -EBUSY;
                 goto out;
         }
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                fmt = find_format(f, MFC_FMT_RAW);
+                if (!fmt) {
+                        mfc_err("Unsupported format for source.\n");
+                        return -EINVAL;
+                }
+                if (!IS_MFCV6(dev) && (fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                } else if (IS_MFCV6(dev) &&
+                                (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
+                ctx->dst_fmt = fmt;
+                mfc_debug_leave();
+                return ret;
+        } else if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                mfc_err("Wrong type error for S_FMT : %d", f->type);
+                return -EINVAL;
+        }
         fmt = find_format(f, MFC_FMT_DEC);
-        if (!fmt || fmt->codec_mode == S5P_FIMV_CODEC_NONE) {
+        if (!fmt || fmt->codec_mode == S5P_MFC_CODEC_NONE) {
                 mfc_err("Unknown codec\n");
                 ret = -EINVAL;
                 goto out;
@@ -391,6 +456,10 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 ret = -EINVAL;
                 goto out;
         }
+        if (!IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
+                mfc_err("Not supported format.\n");
+                return -EINVAL;
+        }
         ctx->src_fmt = fmt;
         ctx->codec_mode = fmt->codec_mode;
         mfc_debug(2, "The codec number is: %d\n", ctx->codec_mode);
@@ -476,7 +545,7 @@ static int vidioc_reqbufs(struct file *file, void *priv,
                         return -ENOMEM;
                 }
                 ctx->total_dpb_count = reqbufs->count;
-                ret = s5p_mfc_alloc_codec_buffers(ctx);
+                ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_codec_buffers, ctx);
                 if (ret) {
                         mfc_err("Failed to allocate decoding buffers\n");
                         reqbufs->count = 0;
@@ -492,15 +561,16 @@ static int vidioc_reqbufs(struct file *file, void *priv,
                         reqbufs->count = 0;
                         s5p_mfc_clock_on();
                         ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-                        s5p_mfc_release_codec_buffers(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers,
+                                        ctx);
                         s5p_mfc_clock_off();
                         return -ENOMEM;
                 }
                 if (s5p_mfc_ctx_ready(ctx))
                         set_work_bit_irqsave(ctx);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 s5p_mfc_wait_for_done_ctx(ctx,
-                                         S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET, 0);
+                                        S5P_MFC_R2H_CMD_INIT_BUFFERS_RET, 0);
         }
         return ret;
 }
@@ -582,18 +652,22 @@ static int vidioc_streamon(struct file *file, void *priv,
                         ctx->src_bufs_cnt = 0;
                         ctx->capture_state = QUEUE_FREE;
                         ctx->output_state = QUEUE_FREE;
-                        s5p_mfc_alloc_instance_buffer(ctx);
-                        s5p_mfc_alloc_dec_temp_buffers(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer,
+                                        ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, alloc_dec_temp_buffers,
+                                        ctx);
                         set_work_bit_irqsave(ctx);
                         s5p_mfc_clean_ctx_int_flags(ctx);
-                        s5p_mfc_try_run(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 
                         if (s5p_mfc_wait_for_done_ctx(ctx,
-                                S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
+                                S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
                                 /* Error or timeout */
                                 mfc_err("Error getting instance from hardware\n");
-                                s5p_mfc_release_instance_buffer(ctx);
-                                s5p_mfc_release_dec_desc_buffer(ctx);
+                                s5p_mfc_hw_call(dev->mfc_ops,
+                                                release_instance_buffer, ctx);
+                                s5p_mfc_hw_call(dev->mfc_ops,
+                                                release_dec_desc_buffer, ctx);
                                 return -EIO;
                         }
                         mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
@@ -662,7 +736,7 @@ static int s5p_mfc_dec_g_v_ctrl(struct v4l2_ctrl *ctrl)
                 /* Should wait for the header to be parsed */
                 s5p_mfc_clean_ctx_int_flags(ctx);
                 s5p_mfc_wait_for_done_ctx(ctx,
-                                S5P_FIMV_R2H_CMD_SEQ_DONE_RET, 0);
+                                S5P_MFC_R2H_CMD_SEQ_DONE_RET, 0);
                 if (ctx->state >= MFCINST_HEAD_PARSED &&
                     ctx->state < MFCINST_ABORT) {
                         ctrl->val = ctx->dpb_count;
@@ -686,6 +760,7 @@ static int vidioc_g_crop(struct file *file, void *priv,
                 struct v4l2_crop *cr)
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
         u32 left, right, top, bottom;
 
         if (ctx->state != MFCINST_HEAD_PARSED &&
@@ -695,10 +770,10 @@ static int vidioc_g_crop(struct file *file, void *priv,
                         return -EINVAL;
                 }
         if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_H264) {
-                left = s5p_mfc_read_shm(ctx, CROP_INFO_H);
+                left = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_h, ctx);
                 right = left >> S5P_FIMV_SHARED_CROP_RIGHT_SHIFT;
                 left = left & S5P_FIMV_SHARED_CROP_LEFT_MASK;
-                top = s5p_mfc_read_shm(ctx, CROP_INFO_V);
+                top = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_v, ctx);
                 bottom = top >> S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT;
                 top = top & S5P_FIMV_SHARED_CROP_TOP_MASK;
                 cr->c.left = left;
@@ -749,6 +824,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                         void *allocators[])
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
 
         /* Video output for decoding (source)
          * this can be set after getting an instance */
@@ -784,7 +860,13 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
             vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
                 psize[0] = ctx->luma_size;
                 psize[1] = ctx->chroma_size;
-                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+
+                if (IS_MFCV6(dev))
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+                else
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
                 allocators[1] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
         } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
                    ctx->state == MFCINST_INIT) {
@@ -876,7 +958,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
         /* If context is ready then dev = work->data;schedule it to run */
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         return 0;
 }
 
@@ -892,19 +974,21 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
                 dev->curr_ctx == ctx->num && dev->hw_lock) {
                 ctx->state = MFCINST_ABORT;
                 s5p_mfc_wait_for_done_ctx(ctx,
-                                        S5P_FIMV_R2H_CMD_FRAME_DONE_RET, 0);
+                                        S5P_MFC_R2H_CMD_FRAME_DONE_RET, 0);
                 aborted = 1;
         }
         spin_lock_irqsave(&dev->irqlock, flags);
         if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
                 INIT_LIST_HEAD(&ctx->dst_queue);
                 ctx->dst_queue_cnt = 0;
                 ctx->dpb_flush_flag = 1;
                 ctx->dec_dst_flag = 0;
         }
         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 INIT_LIST_HEAD(&ctx->src_queue);
                 ctx->src_queue_cnt = 0;
         }
@@ -944,7 +1028,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
         }
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_dec_qops = {
@@ -1028,3 +1112,13 @@ void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx)
                 ctx->ctrls[i] = NULL;
 }
 
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
+{
+        struct v4l2_format f;
+        f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_DEC;
+        ctx->src_fmt = find_format(&f, MFC_FMT_DEC);
+        f.fmt.pix_mp.pixelformat = DEF_DST_FMT_DEC;
+        ctx->dst_fmt = find_format(&f, MFC_FMT_RAW);
+        mfc_debug(2, "Default src_fmt is %x, dest_fmt is %x\n",
+                        (unsigned int)ctx->src_fmt, (unsigned int)ctx->dst_fmt);
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
index fdf1d99a9d15..d06a7cab5eb1 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
@@ -19,5 +19,6 @@ const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_dec_def_fmt(bool src);
 int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx);
 void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx);
 
 #endif /* S5P_MFC_DEC_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index 179e4db60b15..2af6d522f4ac 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -25,48 +25,64 @@
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 
+#define DEF_SRC_FMT_ENC V4L2_PIX_FMT_NV12MT
+#define DEF_DST_FMT_ENC V4L2_PIX_FMT_H264
+
 static struct s5p_mfc_fmt formats[] = {
         {
-                .name = "4:2:0 2 Planes 64x32 Tiles",
-                .fourcc = V4L2_PIX_FMT_NV12MT,
-                .codec_mode = S5P_FIMV_CODEC_NONE,
-                .type = MFC_FMT_RAW,
-                .num_planes = 2,
+                .name           = "4:2:0 2 Planes 16x16 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT_16X16,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
+                .name           = "4:2:0 2 Planes 64x32 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
+                .name           = "4:2:0 2 Planes Y/CbCr",
+                .fourcc         = V4L2_PIX_FMT_NV12M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
         },
         {
-                .name = "4:2:0 2 Planes",
-                .fourcc = V4L2_PIX_FMT_NV12M,
-                .codec_mode = S5P_FIMV_CODEC_NONE,
-                .type = MFC_FMT_RAW,
-                .num_planes = 2,
+                .name           = "4:2:0 2 Planes Y/CrCb",
+                .fourcc         = V4L2_PIX_FMT_NV21M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
         },
         {
-                .name = "H264 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H264,
-                .codec_mode = S5P_FIMV_CODEC_H264_ENC,
-                .type = MFC_FMT_ENC,
-                .num_planes = 1,
+                .name           = "H264 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H264,
+                .codec_mode     = S5P_MFC_CODEC_H264_ENC,
+                .type           = MFC_FMT_ENC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG4 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG4,
-                .codec_mode = S5P_FIMV_CODEC_MPEG4_ENC,
-                .type = MFC_FMT_ENC,
-                .num_planes = 1,
+                .name           = "MPEG4 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG4,
+                .codec_mode     = S5P_MFC_CODEC_MPEG4_ENC,
+                .type           = MFC_FMT_ENC,
+                .num_planes     = 1,
         },
         {
-                .name = "H263 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H263,
-                .codec_mode = S5P_FIMV_CODEC_H263_ENC,
-                .type = MFC_FMT_ENC,
-                .num_planes = 1,
+                .name           = "H263 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H263,
+                .codec_mode     = S5P_MFC_CODEC_H263_ENC,
+                .type           = MFC_FMT_ENC,
+                .num_planes     = 1,
         },
 };
 
@@ -574,7 +590,8 @@ static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
         if (ctx->state == MFCINST_GOT_INST && ctx->dst_queue_cnt >= 1)
                 return 1;
         /* context is ready to encode a frame */
-        if (ctx->state == MFCINST_RUNNING &&
+        if ((ctx->state == MFCINST_RUNNING ||
+                ctx->state == MFCINST_HEAD_PARSED) &&
                 ctx->src_queue_cnt >= 1 && ctx->dst_queue_cnt >= 1)
                 return 1;
         /* context is ready to encode remaining frames */
@@ -619,7 +636,8 @@ static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
         dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
         dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
         dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+                        dst_size);
         spin_unlock_irqrestore(&dev->irqlock, flags);
         return 0;
 }
@@ -638,14 +656,23 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
                 list_del(&dst_mb->list);
                 ctx->dst_queue_cnt--;
                 vb2_set_plane_payload(dst_mb->b, 0,
-                                                s5p_mfc_get_enc_strm_size());
+                        s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev));
                 vb2_buffer_done(dst_mb->b, VB2_BUF_STATE_DONE);
                 spin_unlock_irqrestore(&dev->irqlock, flags);
         }
-        ctx->state = MFCINST_RUNNING;
-        if (s5p_mfc_ctx_ready(ctx))
-                set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        if (IS_MFCV6(dev)) {
+                ctx->state = MFCINST_HEAD_PARSED; /* for INIT_BUFFER cmd */
+        } else {
+                ctx->state = MFCINST_RUNNING;
+                if (s5p_mfc_ctx_ready(ctx))
+                        set_work_bit_irqsave(ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+        }
+
+        if (IS_MFCV6(dev))
+                ctx->dpb_count = s5p_mfc_hw_call(dev->mfc_ops,
+                                get_enc_dpb_count, dev);
+
         return 0;
 }
 
@@ -662,14 +689,16 @@ static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
         src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
         src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
         src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
-        s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr, src_c_addr);
+        s5p_mfc_hw_call(dev->mfc_ops, set_enc_frame_buffer, ctx, src_y_addr,
+                        src_c_addr);
         spin_unlock_irqrestore(&dev->irqlock, flags);
 
         spin_lock_irqsave(&dev->irqlock, flags);
         dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
         dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
         dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+                        dst_size);
         spin_unlock_irqrestore(&dev->irqlock, flags);
 
         return 0;
@@ -685,15 +714,16 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
         unsigned int strm_size;
         unsigned long flags;
 
-        slice_type = s5p_mfc_get_enc_slice_type();
-        strm_size = s5p_mfc_get_enc_strm_size();
+        slice_type = s5p_mfc_hw_call(dev->mfc_ops, get_enc_slice_type, dev);
+        strm_size = s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev);
         mfc_debug(2, "Encoded slice type: %d", slice_type);
         mfc_debug(2, "Encoded stream size: %d", strm_size);
         mfc_debug(2, "Display order: %d",
                   mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
         spin_lock_irqsave(&dev->irqlock, flags);
         if (slice_type >= 0) {
-                s5p_mfc_get_enc_frame_buffer(ctx, &enc_y_addr, &enc_c_addr);
+                s5p_mfc_hw_call(dev->mfc_ops, get_enc_frame_buffer, ctx,
+                                &enc_y_addr, &enc_c_addr);
                 list_for_each_entry(mb_entry, &ctx->src_queue, list) {
                         mb_y_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 0);
                         mb_c_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 1);
@@ -939,15 +969,16 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 pix_fmt_mp->plane_fmt[0].bytesperline = 0;
                 ctx->dst_bufs_cnt = 0;
                 ctx->capture_state = QUEUE_FREE;
-                s5p_mfc_alloc_instance_buffer(ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer, ctx);
                 set_work_bit_irqsave(ctx);
                 s5p_mfc_clean_ctx_int_flags(ctx);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 if (s5p_mfc_wait_for_done_ctx(ctx, \
-                                S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
+                                S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
                                 /* Error or timeout */
                         mfc_err("Error getting instance from hardware\n");
-                        s5p_mfc_release_instance_buffer(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer,
+                                        ctx);
                         ret = -EIO;
                         goto out;
                 }
@@ -958,6 +989,17 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                         mfc_err("failed to set output format\n");
                         return -EINVAL;
                 }
+
+                if (!IS_MFCV6(dev) &&
+                                (fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                } else if (IS_MFCV6(dev) &&
+                                (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
+
                 if (fmt->num_planes != pix_fmt_mp->num_planes) {
                         mfc_err("failed to set output format\n");
                         ret = -EINVAL;
@@ -970,45 +1012,13 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 mfc_debug(2, "fmt - w: %d, h: %d, ctx - w: %d, h: %d\n",
                         pix_fmt_mp->width, pix_fmt_mp->height,
                         ctx->img_width, ctx->img_height);
-                if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
-                        ctx->buf_width = ALIGN(ctx->img_width,
-                                                        S5P_FIMV_NV12M_HALIGN);
-                        ctx->luma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12M_HALIGN) * ALIGN(ctx->img_height,
-                                S5P_FIMV_NV12M_LVALIGN);
-                        ctx->chroma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12M_HALIGN) * ALIGN((ctx->img_height
-                                >> 1), S5P_FIMV_NV12M_CVALIGN);
-
-                        ctx->luma_size = ALIGN(ctx->luma_size,
-                                                        S5P_FIMV_NV12M_SALIGN);
-                        ctx->chroma_size = ALIGN(ctx->chroma_size,
-                                                        S5P_FIMV_NV12M_SALIGN);
-
-                        pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
-                        pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
-                        pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
-                        pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
-
-                } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
-                        ctx->buf_width = ALIGN(ctx->img_width,
-                                                        S5P_FIMV_NV12MT_HALIGN);
-                        ctx->luma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12MT_HALIGN) * ALIGN(ctx->img_height,
-                                S5P_FIMV_NV12MT_VALIGN);
-                        ctx->chroma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12MT_HALIGN) * ALIGN((ctx->img_height
-                                >> 1), S5P_FIMV_NV12MT_VALIGN);
-                        ctx->luma_size = ALIGN(ctx->luma_size,
-                                                        S5P_FIMV_NV12MT_SALIGN);
-                        ctx->chroma_size = ALIGN(ctx->chroma_size,
-                                                        S5P_FIMV_NV12MT_SALIGN);
-
-                        pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
-                        pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
-                        pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
-                        pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
-                }
+
+                s5p_mfc_hw_call(dev->mfc_ops, enc_calc_src_size, ctx);
+                pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+                pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+                pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+                pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+
                 ctx->src_bufs_cnt = 0;
                 ctx->output_state = QUEUE_FREE;
         } else {
@@ -1023,6 +1033,7 @@ out:
 static int vidioc_reqbufs(struct file *file, void *priv,
                                           struct v4l2_requestbuffers *reqbufs)
 {
+        struct s5p_mfc_dev *dev = video_drvdata(file);
         struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
         int ret = 0;
 
@@ -1042,12 +1053,16 @@ static int vidioc_reqbufs(struct file *file, void *priv,
                         return ret;
                 }
                 ctx->capture_state = QUEUE_BUFS_REQUESTED;
-                ret = s5p_mfc_alloc_codec_buffers(ctx);
-                if (ret) {
-                        mfc_err("Failed to allocate encoding buffers\n");
-                        reqbufs->count = 0;
-                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-                        return -ENOMEM;
+
+                if (!IS_MFCV6(dev)) {
+                        ret = s5p_mfc_hw_call(ctx->dev->mfc_ops,
+                                        alloc_codec_buffers, ctx);
+                        if (ret) {
+                                mfc_err("Failed to allocate encoding buffers\n");
+                                reqbufs->count = 0;
+                                ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                                return -ENOMEM;
+                        }
                 }
         } else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
                 if (ctx->output_state != QUEUE_FREE) {
@@ -1310,6 +1325,13 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
                         p->codec.h264.profile =
                                 S5P_FIMV_ENC_PROFILE_H264_BASELINE;
                         break;
+                case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+                        if (IS_MFCV6(dev))
+                                p->codec.h264.profile =
+                                S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE;
+                        else
+                                ret = -EINVAL;
+                        break;
                 default:
                         ret = -EINVAL;
                 }
@@ -1349,7 +1371,7 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
                 p->codec.h264._8x8_transform = ctrl->val;
                 break;
         case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
-                p->codec.h264.rc_mb = ctrl->val;
+                p->rc_mb = ctrl->val;
                 break;
         case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
                 p->codec.h264.rc_frame_qp = ctrl->val;
@@ -1500,7 +1522,7 @@ int vidioc_encoder_cmd(struct file *file, void *priv,
                         mfc_debug(2, "EOS: empty src queue, entering finishing state");
                         ctx->state = MFCINST_FINISHING;
                         spin_unlock_irqrestore(&dev->irqlock, flags);
-                        s5p_mfc_try_run(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 } else {
                         mfc_debug(2, "EOS: marking last buffer of stream");
                         buf = list_entry(ctx->src_queue.prev,
@@ -1583,6 +1605,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                         unsigned int psize[], void *allocators[])
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
 
         if (ctx->state != MFCINST_GOT_INST) {
                 mfc_err("inavlid state: %d\n", ctx->state);
@@ -1611,8 +1634,17 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                         *buf_count = MFC_MAX_BUFFERS;
                 psize[0] = ctx->luma_size;
                 psize[1] = ctx->chroma_size;
-                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
-                allocators[1] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                if (IS_MFCV6(dev)) {
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+                        allocators[1] =
+                                ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+                } else {
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                        allocators[1] =
+                                ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                }
         } else {
                 mfc_err("inavlid queue type: %d\n", vq->type);
                 return -EINVAL;
@@ -1715,7 +1747,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
         /* If context is ready then dev = work->data;schedule it to run */
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         return 0;
 }
 
@@ -1729,19 +1761,21 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
                 ctx->state == MFCINST_RUNNING) &&
                 dev->curr_ctx == ctx->num && dev->hw_lock) {
                 ctx->state = MFCINST_ABORT;
-                s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_FRAME_DONE_RET,
+                s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_FRAME_DONE_RET,
                                           0);
         }
         ctx->state = MFCINST_FINISHED;
         spin_lock_irqsave(&dev->irqlock, flags);
         if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
                 INIT_LIST_HEAD(&ctx->dst_queue);
                 ctx->dst_queue_cnt = 0;
         }
         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
                 cleanup_ref_queue(ctx);
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 INIT_LIST_HEAD(&ctx->src_queue);
                 ctx->src_queue_cnt = 0;
         }
@@ -1782,7 +1816,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
         }
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_enc_qops = {
@@ -1880,3 +1914,13 @@ void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx)
         for (i = 0; i < NUM_CTRLS; i++)
                 ctx->ctrls[i] = NULL;
 }
+
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx)
+{
+        struct v4l2_format f;
+        f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_ENC;
+        ctx->src_fmt = find_format(&f, MFC_FMT_RAW);
+        f.fmt.pix_mp.pixelformat = DEF_DST_FMT_ENC;
+        ctx->dst_fmt = find_format(&f, MFC_FMT_ENC);
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
index ca9fd66bd310..5118d46b3a9e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
@@ -19,5 +19,6 @@ const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_enc_def_fmt(bool src);
 int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx);
 void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx);
 
 #endif /* S5P_MFC_ENC_H_  */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
index 37860e299021..5b8f0e085e6d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
@@ -17,7 +17,6 @@
 #include <linux/io.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
@@ -28,7 +27,7 @@ int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
 
         ret = wait_event_interruptible_timeout(dev->queue,
                 (dev->int_cond && (dev->int_type == command
-                || dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                || dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
                 msecs_to_jiffies(MFC_INT_TIMEOUT));
         if (ret == 0) {
                 mfc_err("Interrupt (dev->int_type:%d, command:%d) timed out\n",
@@ -40,7 +39,7 @@ int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
         }
         mfc_debug(1, "Finished waiting (dev->int_type:%d, command: %d)\n",
                                                         dev->int_type, command);
-        if (dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+        if (dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)
                 return 1;
         return 0;
 }
@@ -60,12 +59,12 @@ int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
         if (interrupt) {
                 ret = wait_event_interruptible_timeout(ctx->queue,
                                 (ctx->int_cond && (ctx->int_type == command
-                        || ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                        || ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
                                         msecs_to_jiffies(MFC_INT_TIMEOUT));
         } else {
                 ret = wait_event_timeout(ctx->queue,
                                 (ctx->int_cond && (ctx->int_type == command
-                        || ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                        || ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
                                         msecs_to_jiffies(MFC_INT_TIMEOUT));
         }
         if (ret == 0) {
@@ -78,7 +77,7 @@ int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
         }
         mfc_debug(1, "Finished waiting (ctx->int_type:%d, command: %d)\n",
                                                         ctx->int_type, command);
-        if (ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+        if (ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)
                 return 1;
         return 0;
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
index 767a51271dc2..6932e90d4065 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -1,10 +1,10 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_opr.c
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
  *
  * Samsung MFC (Multi Function Codec - FIMV) driver
  * This file contains hw related functions.
  *
- * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * Kamil Debski, Copyright (c) 2012 Samsung Electronics Co., Ltd.
  * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -12,1414 +12,20 @@
  * published by the Free Software Foundation.
  */
 
-#include "regs-mfc.h"
-#include "s5p_mfc_cmd.h"
-#include "s5p_mfc_common.h"
-#include "s5p_mfc_ctrl.h"
-#include "s5p_mfc_debug.h"
-#include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
-#include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
-#include <asm/cacheflush.h>
-#include <linux/delay.h>
-#include <linux/dma-mapping.h>
-#include <linux/err.h>
-#include <linux/firmware.h>
-#include <linux/io.h>
-#include <linux/jiffies.h>
-#include <linux/mm.h>
-#include <linux/sched.h>
+#include "s5p_mfc_opr_v5.h"
+#include "s5p_mfc_opr_v6.h"
 
-#define OFFSETA(x)              (((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
-#define OFFSETB(x)              (((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+static struct s5p_mfc_hw_ops *s5p_mfc_ops;
 
-/* Allocate temporary buffers for decoding */
-int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev)
 {
-        void *desc_virt;
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        ctx->desc_buf = vb2_dma_contig_memops.alloc(
-                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], DESC_BUF_SIZE);
-        if (IS_ERR_VALUE((int)ctx->desc_buf)) {
-                ctx->desc_buf = NULL;
-                mfc_err("Allocating DESC buffer failed\n");
-                return -ENOMEM;
-        }
-        ctx->desc_phys = s5p_mfc_mem_cookie(
-                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->desc_buf);
-        BUG_ON(ctx->desc_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        desc_virt = vb2_dma_contig_memops.vaddr(ctx->desc_buf);
-        if (desc_virt == NULL) {
-                vb2_dma_contig_memops.put(ctx->desc_buf);
-                ctx->desc_phys = 0;
-                ctx->desc_buf = NULL;
-                mfc_err("Remapping DESC buffer failed\n");
-                return -ENOMEM;
-        }
-        memset(desc_virt, 0, DESC_BUF_SIZE);
-        wmb();
-        return 0;
-}
-
-/* Release temporary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
-{
-        if (ctx->desc_phys) {
-                vb2_dma_contig_memops.put(ctx->desc_buf);
-                ctx->desc_phys = 0;
-                ctx->desc_buf = NULL;
-        }
-}
-
-/* Allocate codec buffers */
-int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned int enc_ref_y_size = 0;
-        unsigned int enc_ref_c_size = 0;
-        unsigned int guard_width, guard_height;
-
-        if (ctx->type == MFCINST_DECODER) {
-                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
-                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
-                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
-        } else if (ctx->type == MFCINST_ENCODER) {
-                enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
-                enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
-
-                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
-                        enc_ref_c_size = ALIGN(ctx->img_width,
-                                                S5P_FIMV_NV12MT_HALIGN)
-                                                * ALIGN(ctx->img_height >> 1,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                        enc_ref_c_size = ALIGN(enc_ref_c_size,
-                                                        S5P_FIMV_NV12MT_SALIGN);
-                } else {
-                        guard_width = ALIGN(ctx->img_width + 16,
-                                                        S5P_FIMV_NV12MT_HALIGN);
-                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
-                                                        S5P_FIMV_NV12MT_VALIGN);
-                        enc_ref_c_size = ALIGN(guard_width * guard_height,
-                                               S5P_FIMV_NV12MT_SALIGN);
-                }
-                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
-                          enc_ref_y_size, enc_ref_c_size);
+        if (IS_MFCV6(dev)) {
+                s5p_mfc_ops = s5p_mfc_init_hw_ops_v6();
+                dev->warn_start = S5P_FIMV_ERR_WARNINGS_START_V6;
         } else {
-                return -EINVAL;
-        }
-        /* Codecs have different memory requirements */
-        switch (ctx->codec_mode) {
-        case S5P_FIMV_CODEC_H264_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
-                                        S5P_FIMV_DEC_VERT_NB_MV_SIZE,
-                                        S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
-                break;
-        case S5P_FIMV_CODEC_MPEG4_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
-                                     S5P_FIMV_DEC_UPNB_MV_SIZE +
-                                     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-                                     S5P_FIMV_DEC_STX_PARSER_SIZE +
-                                     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
-                                     S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_VC1RCV_DEC:
-        case S5P_FIMV_CODEC_VC1_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
-                             S5P_FIMV_DEC_UPNB_MV_SIZE +
-                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-                             S5P_FIMV_DEC_NB_DCAC_SIZE +
-                             3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
-                             S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_MPEG2_DEC:
-                ctx->bank1_size = 0;
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_H263_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
-                             S5P_FIMV_DEC_UPNB_MV_SIZE +
-                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-                             S5P_FIMV_DEC_NB_DCAC_SIZE,
-                             S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_H264_ENC:
-                ctx->bank1_size = (enc_ref_y_size * 2) +
-                                   S5P_FIMV_ENC_UPMV_SIZE +
-                                   S5P_FIMV_ENC_COLFLG_SIZE +
-                                   S5P_FIMV_ENC_INTRAMD_SIZE +
-                                   S5P_FIMV_ENC_NBORINFO_SIZE;
-                ctx->bank2_size = (enc_ref_y_size * 2) +
-                                   (enc_ref_c_size * 4) +
-                                   S5P_FIMV_ENC_INTRAPRED_SIZE;
-                break;
-        case S5P_FIMV_CODEC_MPEG4_ENC:
-                ctx->bank1_size = (enc_ref_y_size * 2) +
-                                   S5P_FIMV_ENC_UPMV_SIZE +
-                                   S5P_FIMV_ENC_COLFLG_SIZE +
-                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                ctx->bank2_size = (enc_ref_y_size * 2) +
-                                   (enc_ref_c_size * 4);
-                break;
-        case S5P_FIMV_CODEC_H263_ENC:
-                ctx->bank1_size = (enc_ref_y_size * 2) +
-                                   S5P_FIMV_ENC_UPMV_SIZE +
-                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                ctx->bank2_size = (enc_ref_y_size * 2) +
-                                   (enc_ref_c_size * 4);
-                break;
-        default:
-                break;
-        }
-        /* Allocate only if memory from bank 1 is necessary */
-        if (ctx->bank1_size > 0) {
-                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
-                if (IS_ERR(ctx->bank1_buf)) {
-                        ctx->bank1_buf = NULL;
-                        printk(KERN_ERR
-                               "Buf alloc for decoding failed (port A)\n");
-                        return -ENOMEM;
-                }
-                ctx->bank1_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
-                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        }
-        /* Allocate only if memory from bank 2 is necessary */
-        if (ctx->bank2_size > 0) {
-                ctx->bank2_buf = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
-                if (IS_ERR(ctx->bank2_buf)) {
-                        ctx->bank2_buf = NULL;
-                        mfc_err("Buf alloc for decoding failed (port B)\n");
-                        return -ENOMEM;
-                }
-                ctx->bank2_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
-                BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
-        }
-        return 0;
-}
-
-/* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx)
-{
-        if (ctx->bank1_buf) {
-                vb2_dma_contig_memops.put(ctx->bank1_buf);
-                ctx->bank1_buf = NULL;
-                ctx->bank1_phys = 0;
-                ctx->bank1_size = 0;
-        }
-        if (ctx->bank2_buf) {
-                vb2_dma_contig_memops.put(ctx->bank2_buf);
-                ctx->bank2_buf = NULL;
-                ctx->bank2_phys = 0;
-                ctx->bank2_size = 0;
+                s5p_mfc_ops = s5p_mfc_init_hw_ops_v5();
+                dev->warn_start = S5P_FIMV_ERR_WARNINGS_START;
         }
+        dev->mfc_ops = s5p_mfc_ops;
 }
-
-/* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
-{
-        void *context_virt;
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
-                ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
-                ctx->ctx_size = MFC_H264_CTX_BUF_SIZE;
-        else
-                ctx->ctx_size = MFC_CTX_BUF_SIZE;
-        ctx->ctx_buf = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_size);
-        if (IS_ERR(ctx->ctx_buf)) {
-                mfc_err("Allocating context buffer failed\n");
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-                return -ENOMEM;
-        }
-        ctx->ctx_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_buf);
-        BUG_ON(ctx->ctx_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        ctx->ctx_ofs = OFFSETA(ctx->ctx_phys);
-        context_virt = vb2_dma_contig_memops.vaddr(ctx->ctx_buf);
-        if (context_virt == NULL) {
-                mfc_err("Remapping instance buffer failed\n");
-                vb2_dma_contig_memops.put(ctx->ctx_buf);
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-                return -ENOMEM;
-        }
-        /* Zero content of the allocated memory */
-        memset(context_virt, 0, ctx->ctx_size);
-        wmb();
-        if (s5p_mfc_init_shm(ctx) < 0) {
-                vb2_dma_contig_memops.put(ctx->ctx_buf);
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-                return -ENOMEM;
-        }
-        return 0;
-}
-
-/* Release instance buffer */
-void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx)
-{
-        if (ctx->ctx_buf) {
-                vb2_dma_contig_memops.put(ctx->ctx_buf);
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-        }
-        if (ctx->shm_alloc) {
-                vb2_dma_contig_memops.put(ctx->shm_alloc);
-                ctx->shm_alloc = NULL;
-                ctx->shm = NULL;
-        }
-}
-
-/* Set registers for decoding temporary buffers */
-void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETA(ctx->desc_phys), S5P_FIMV_SI_CH0_DESC_ADR);
-        mfc_write(dev, DESC_BUF_SIZE, S5P_FIMV_SI_CH0_DESC_SIZE);
-}
-
-/* Set registers for shared buffer */
-static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        mfc_write(dev, ctx->shm_ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
-}
-
-/* Set registers for decoding stream buffer */
-int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
-                  unsigned int start_num_byte, unsigned int buf_size)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
-        mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
-        mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
-        s5p_mfc_write_shm(ctx, start_num_byte, START_BYTE_NUM);
-        return 0;
-}
-
-/* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx)
-{
-        unsigned int frame_size, i;
-        unsigned int frame_size_ch, frame_size_mv;
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned int dpb;
-        size_t buf_addr1, buf_addr2;
-        int buf_size1, buf_size2;
-
-        buf_addr1 = ctx->bank1_phys;
-        buf_size1 = ctx->bank1_size;
-        buf_addr2 = ctx->bank2_phys;
-        buf_size2 = ctx->bank2_size;
-        dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
-                                                ~S5P_FIMV_DPB_COUNT_MASK;
-        mfc_write(dev, ctx->total_dpb_count | dpb,
-                                                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-        s5p_mfc_set_shared_buffer(ctx);
-        switch (ctx->codec_mode) {
-        case S5P_FIMV_CODEC_H264_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                                S5P_FIMV_H264_VERT_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
-                break;
-        case S5P_FIMV_CODEC_MPEG4_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
-                buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
-                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                break;
-        case S5P_FIMV_CODEC_H263_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
-                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-                break;
-        case S5P_FIMV_CODEC_VC1_DEC:
-        case S5P_FIMV_CODEC_VC1RCV_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
-                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                break;
-        case S5P_FIMV_CODEC_MPEG2_DEC:
-                break;
-        default:
-                mfc_err("Unknown codec for decoding (%x)\n",
-                        ctx->codec_mode);
-                return -EINVAL;
-                break;
-        }
-        frame_size = ctx->luma_size;
-        frame_size_ch = ctx->chroma_size;
-        frame_size_mv = ctx->mv_size;
-        mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
-                                                                frame_size_mv);
-        for (i = 0; i < ctx->total_dpb_count; i++) {
-                /* Bank2 */
-                mfc_debug(2, "Luma %d: %x\n", i,
-                                        ctx->dst_bufs[i].cookie.raw.luma);
-                mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
-                                                S5P_FIMV_DEC_LUMA_ADR + i * 4);
-                mfc_debug(2, "\tChroma %d: %x\n", i,
-                                        ctx->dst_bufs[i].cookie.raw.chroma);
-                mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
-                                               S5P_FIMV_DEC_CHROMA_ADR + i * 4);
-                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
-                        mfc_debug(2, "\tBuf2: %x, size: %d\n",
-                                                        buf_addr2, buf_size2);
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                                S5P_FIMV_H264_MV_ADR + i * 4);
-                        buf_addr2 += frame_size_mv;
-                        buf_size2 -= frame_size_mv;
-                }
-        }
-        mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
-        mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
-                        buf_size1,  buf_size2, ctx->total_dpb_count);
-        if (buf_size1 < 0 || buf_size2 < 0) {
-                mfc_debug(2, "Not enough memory has been allocated\n");
-                return -ENOMEM;
-        }
-        s5p_mfc_write_shm(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
-        s5p_mfc_write_shm(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC)
-                s5p_mfc_write_shm(ctx, frame_size_mv, ALLOC_MV_SIZE);
-        mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
-                                        << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
-                                                S5P_FIMV_SI_CH0_INST_ID);
-        return 0;
-}
-
-/* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long addr, unsigned int size)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
-        mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
-        return 0;
-}
-
-void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long y_addr, unsigned long c_addr)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
-        mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
-}
-
-void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long *y_addr, unsigned long *c_addr)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        *y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
-                                                        << MFC_OFFSET_SHIFT);
-        *c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
-                                                        << MFC_OFFSET_SHIFT);
-}
-
-/* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        size_t buf_addr1, buf_addr2;
-        size_t buf_size1, buf_size2;
-        unsigned int enc_ref_y_size, enc_ref_c_size;
-        unsigned int guard_width, guard_height;
-        int i;
-
-        buf_addr1 = ctx->bank1_phys;
-        buf_size1 = ctx->bank1_size;
-        buf_addr2 = ctx->bank2_phys;
-        buf_size2 = ctx->bank2_size;
-        enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-                * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
-        enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
-                enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
-                enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
-        } else {
-                guard_width = ALIGN(ctx->img_width + 16,
-                                                S5P_FIMV_NV12MT_HALIGN);
-                guard_height = ALIGN((ctx->img_height >> 1) + 4,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                enc_ref_c_size = ALIGN(guard_width * guard_height,
-                                       S5P_FIMV_NV12MT_SALIGN);
-        }
-        mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
-        switch (ctx->codec_mode) {
-        case S5P_FIMV_CODEC_H264_ENC:
-                for (i = 0; i < 2; i++) {
-                        mfc_write(dev, OFFSETA(buf_addr1),
-                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-                        buf_addr1 += enc_ref_y_size;
-                        buf_size1 -= enc_ref_y_size;
-
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_y_size;
-                        buf_size2 -= enc_ref_y_size;
-                }
-                for (i = 0; i < 4; i++) {
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_c_size;
-                        buf_size2 -= enc_ref_c_size;
-                }
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
-                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                        S5P_FIMV_H264_COZERO_FLAG_ADR);
-                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                        S5P_FIMV_H264_UP_INTRA_MD_ADR);
-                buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
-                mfc_write(dev, OFFSETB(buf_addr2),
-                                        S5P_FIMV_H264_UP_INTRA_PRED_ADR);
-                buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
-                buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                        S5P_FIMV_H264_NBOR_INFO_ADR);
-                buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
-                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-                        buf_size1, buf_size2);
-                break;
-        case S5P_FIMV_CODEC_MPEG4_ENC:
-                for (i = 0; i < 2; i++) {
-                        mfc_write(dev, OFFSETA(buf_addr1),
-                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-                        buf_addr1 += enc_ref_y_size;
-                        buf_size1 -= enc_ref_y_size;
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_y_size;
-                        buf_size2 -= enc_ref_y_size;
-                }
-                for (i = 0; i < 4; i++) {
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_c_size;
-                        buf_size2 -= enc_ref_c_size;
-                }
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
-                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                                S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
-                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                                S5P_FIMV_MPEG4_ACDC_COEF_ADR);
-                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-                        buf_size1, buf_size2);
-                break;
-        case S5P_FIMV_CODEC_H263_ENC:
-                for (i = 0; i < 2; i++) {
-                        mfc_write(dev, OFFSETA(buf_addr1),
-                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-                        buf_addr1 += enc_ref_y_size;
-                        buf_size1 -= enc_ref_y_size;
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_y_size;
-                        buf_size2 -= enc_ref_y_size;
-                }
-                for (i = 0; i < 4; i++) {
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_c_size;
-                        buf_size2 -= enc_ref_c_size;
-                }
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
-                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
-                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-                        buf_size1, buf_size2);
-                break;
-        default:
-                mfc_err("Unknown codec set for encoding: %d\n",
-                        ctx->codec_mode);
-                return -EINVAL;
-        }
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        unsigned int reg;
-        unsigned int shm;
-
-        /* width */
-        mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
-        /* height */
-        mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
-        /* pictype : enable, IDR period */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        reg |= (1 << 18);
-        reg &= ~(0xFFFF);
-        reg |= p->gop_size;
-        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
-        /* multi-slice control */
-        /* multi-slice MB number or bit size */
-        mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
-        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
-                mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
-        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
-                mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
-        } else {
-                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
-                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
-        }
-        /* cyclic intra refresh */
-        mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
-        /* memory structure cur. frame */
-        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
-                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
-        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
-                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
-        /* padding control & value */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
-        if (p->pad) {
-                /** enable */
-                reg |= (1 << 31);
-                /** cr value */
-                reg &= ~(0xFF << 16);
-                reg |= (p->pad_cr << 16);
-                /** cb value */
-                reg &= ~(0xFF << 8);
-                reg |= (p->pad_cb << 8);
-                /** y value */
-                reg &= ~(0xFF);
-                reg |= (p->pad_luma);
-        } else {
-                /** disable & all value clear */
-                reg = 0;
-        }
-        mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /** frame-level rate control */
-        reg &= ~(0x1 << 9);
-        reg |= (p->rc_frame << 9);
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* bit rate */
-        if (p->rc_frame)
-                mfc_write(dev, p->rc_bitrate,
-                        S5P_FIMV_ENC_RC_BIT_RATE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
-        /* reaction coefficient */
-        if (p->rc_frame)
-                mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* seq header ctrl */
-        shm &= ~(0x1 << 3);
-        shm |= (p->seq_hdr_mode << 3);
-        /* frame skip mode */
-        shm &= ~(0x3 << 1);
-        shm |= (p->frame_skip_mode << 1);
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        /* fixed target bit */
-        s5p_mfc_write_shm(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
-        unsigned int reg;
-        unsigned int shm;
-
-        s5p_mfc_set_enc_params(ctx);
-        /* pictype : number of B */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* num_b_frame - 0 ~ 2 */
-        reg &= ~(0x3 << 16);
-        reg |= (p->num_b_frame << 16);
-        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* profile & level */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
-        /* level */
-        reg &= ~(0xFF << 8);
-        reg |= (p_264->level << 8);
-        /* profile - 0 ~ 2 */
-        reg &= ~(0x3F);
-        reg |= p_264->profile;
-        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
-        /* interlace  */
-        mfc_write(dev, p->interlace, S5P_FIMV_ENC_PIC_STRUCT);
-        /* height */
-        if (p->interlace)
-                mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
-        /* loopfilter ctrl */
-        mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
-        /* loopfilter alpha offset */
-        if (p_264->loop_filter_alpha < 0) {
-                reg = 0x10;
-                reg |= (0xFF - p_264->loop_filter_alpha) + 1;
-        } else {
-                reg = 0x00;
-                reg |= (p_264->loop_filter_alpha & 0xF);
-        }
-        mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
-        /* loopfilter beta offset */
-        if (p_264->loop_filter_beta < 0) {
-                reg = 0x10;
-                reg |= (0xFF - p_264->loop_filter_beta) + 1;
-        } else {
-                reg = 0x00;
-                reg |= (p_264->loop_filter_beta & 0xF);
-        }
-        mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
-        /* entropy coding mode */
-        if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
-                mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
-        /* number of ref. picture */
-        reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
-        /* num of ref. pictures of P */
-        reg &= ~(0x3 << 5);
-        reg |= (p_264->num_ref_pic_4p << 5);
-        /* max number of ref. pictures */
-        reg &= ~(0x1F);
-        reg |= p_264->max_ref_pic;
-        mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
-        /* 8x8 transform enable */
-        mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /* macroblock level rate control */
-        reg &= ~(0x1 << 8);
-        reg |= (p_264->rc_mb << 8);
-        /* frame QP */
-        reg &= ~(0x3F);
-        reg |= p_264->rc_frame_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* frame rate */
-        if (p->rc_frame && p->rc_framerate_denom)
-                mfc_write(dev, p->rc_framerate_num * 1000
-                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-        /* max & min value of QP */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-        /* max QP */
-        reg &= ~(0x3F << 8);
-        reg |= (p_264->rc_max_qp << 8);
-        /* min QP */
-        reg &= ~(0x3F);
-        reg |= p_264->rc_min_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-        /* macroblock adaptive scaling features */
-        if (p_264->rc_mb) {
-                reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
-                /* dark region */
-                reg &= ~(0x1 << 3);
-                reg |= (p_264->rc_mb_dark << 3);
-                /* smooth region */
-                reg &= ~(0x1 << 2);
-                reg |= (p_264->rc_mb_smooth << 2);
-                /* static region */
-                reg &= ~(0x1 << 1);
-                reg |= (p_264->rc_mb_static << 1);
-                /* high activity region */
-                reg &= ~(0x1);
-                reg |= p_264->rc_mb_activity;
-                mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
-        }
-        if (!p->rc_frame &&
-            !p_264->rc_mb) {
-                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-                shm &= ~(0xFFF);
-                shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
-                shm |= (p_264->rc_p_frame_qp & 0x3F);
-                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-        }
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* AR VUI control */
-        shm &= ~(0x1 << 15);
-        shm |= (p_264->vui_sar << 1);
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        if (p_264->vui_sar) {
-                /* aspect ration IDC */
-                shm = s5p_mfc_read_shm(ctx, SAMPLE_ASPECT_RATIO_IDC);
-                shm &= ~(0xFF);
-                shm |= p_264->vui_sar_idc;
-                s5p_mfc_write_shm(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
-                if (p_264->vui_sar_idc == 0xFF) {
-                        /* sample  AR info */
-                        shm = s5p_mfc_read_shm(ctx, EXTENDED_SAR);
-                        shm &= ~(0xFFFFFFFF);
-                        shm |= p_264->vui_ext_sar_width << 16;
-                        shm |= p_264->vui_ext_sar_height;
-                        s5p_mfc_write_shm(ctx, shm, EXTENDED_SAR);
-                }
-        }
-        /* intra picture period for H.264 */
-        shm = s5p_mfc_read_shm(ctx, H264_I_PERIOD);
-        /* control */
-        shm &= ~(0x1 << 16);
-        shm |= (p_264->open_gop << 16);
-        /* value */
-        if (p_264->open_gop) {
-                shm &= ~(0xFFFF);
-                shm |= p_264->open_gop_size;
-        }
-        s5p_mfc_write_shm(ctx, shm, H264_I_PERIOD);
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* vbv buffer size */
-        if (p->frame_skip_mode ==
-                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-                shm &= ~(0xFFFF << 16);
-                shm |= (p_264->cpb_size << 16);
-        }
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
-        unsigned int reg;
-        unsigned int shm;
-        unsigned int framerate;
-
-        s5p_mfc_set_enc_params(ctx);
-        /* pictype : number of B */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* num_b_frame - 0 ~ 2 */
-        reg &= ~(0x3 << 16);
-        reg |= (p->num_b_frame << 16);
-        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* profile & level */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
-        /* level */
-        reg &= ~(0xFF << 8);
-        reg |= (p_mpeg4->level << 8);
-        /* profile - 0 ~ 2 */
-        reg &= ~(0x3F);
-        reg |= p_mpeg4->profile;
-        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
-        /* quarter_pixel */
-        mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
-        /* qp */
-        if (!p->rc_frame) {
-                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-                shm &= ~(0xFFF);
-                shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
-                shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
-                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-        }
-        /* frame rate */
-        if (p->rc_frame) {
-                if (p->rc_framerate_denom > 0) {
-                        framerate = p->rc_framerate_num * 1000 /
-                                                p->rc_framerate_denom;
-                        mfc_write(dev, framerate,
-                                S5P_FIMV_ENC_RC_FRAME_RATE);
-                        shm = s5p_mfc_read_shm(ctx, RC_VOP_TIMING);
-                        shm &= ~(0xFFFFFFFF);
-                        shm |= (1 << 31);
-                        shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
-                        shm |= (p->rc_framerate_denom & 0xFFFF);
-                        s5p_mfc_write_shm(ctx, shm, RC_VOP_TIMING);
-                }
-        } else {
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-        }
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /* frame QP */
-        reg &= ~(0x3F);
-        reg |= p_mpeg4->rc_frame_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* max & min value of QP */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-        /* max QP */
-        reg &= ~(0x3F << 8);
-        reg |= (p_mpeg4->rc_max_qp << 8);
-        /* min QP */
-        reg &= ~(0x3F);
-        reg |= p_mpeg4->rc_min_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* vbv buffer size */
-        if (p->frame_skip_mode ==
-                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-                shm &= ~(0xFFFF << 16);
-                shm |= (p->vbv_size << 16);
-        }
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
-        unsigned int reg;
-        unsigned int shm;
-
-        s5p_mfc_set_enc_params(ctx);
-        /* qp */
-        if (!p->rc_frame) {
-                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-                shm &= ~(0xFFF);
-                shm |= (p_h263->rc_p_frame_qp & 0x3F);
-                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-        }
-        /* frame rate */
-        if (p->rc_frame && p->rc_framerate_denom)
-                mfc_write(dev, p->rc_framerate_num * 1000
-                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /* frame QP */
-        reg &= ~(0x3F);
-        reg |= p_h263->rc_frame_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* max & min value of QP */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-        /* max QP */
-        reg &= ~(0x3F << 8);
-        reg |= (p_h263->rc_max_qp << 8);
-        /* min QP */
-        reg &= ~(0x3F);
-        reg |= p_h263->rc_min_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* vbv buffer size */
-        if (p->frame_skip_mode ==
-                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-                shm &= ~(0xFFFF << 16);
-                shm |= (p->vbv_size << 16);
-        }
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        return 0;
-}
-
-/* Initialize decoding */
-int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        s5p_mfc_set_shared_buffer(ctx);
-        /* Setup loop filter, for decoding this is only valid for MPEG4 */
-        if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_DEC)
-                mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
-        mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
-                S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
-                S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
-                S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
-                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-        mfc_write(dev,
-        ((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
-                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-        return 0;
-}
-
-static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned int dpb;
-
-        if (flush)
-                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
-                        S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
-        else
-                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
-                        ~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
-        mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-}
-
-/* Decode a single frame */
-int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
-                                        enum s5p_mfc_decode_arg last_frame)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
-        s5p_mfc_set_shared_buffer(ctx);
-        s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
-        /* Issue different commands to instance basing on whether it
-         * is the last frame or not. */
-        switch (last_frame) {
-        case MFC_DEC_FRAME:
-                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
-                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-                break;
-        case MFC_DEC_LAST_FRAME:
-                mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
-                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-                break;
-        case MFC_DEC_RES_CHANGE:
-                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
-                S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
-                S5P_FIMV_SI_CH0_INST_ID);
-                break;
-        }
-        mfc_debug(2, "Decoding a usual frame\n");
-        return 0;
-}
-
-int s5p_mfc_init_encode(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
-                s5p_mfc_set_enc_params_h264(ctx);
-        else if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_ENC)
-                s5p_mfc_set_enc_params_mpeg4(ctx);
-        else if (ctx->codec_mode == S5P_FIMV_CODEC_H263_ENC)
-                s5p_mfc_set_enc_params_h263(ctx);
-        else {
-                mfc_err("Unknown codec for encoding (%x)\n",
-                        ctx->codec_mode);
-                return -EINVAL;
-        }
-        s5p_mfc_set_shared_buffer(ctx);
-        mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
-                (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-        return 0;
-}
-
-/* Encode a single frame */
-int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        int cmd;
-        /* memory structure cur. frame */
-        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
-                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
-        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
-                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
-        s5p_mfc_set_shared_buffer(ctx);
-
-        if (ctx->state == MFCINST_FINISHING)
-                cmd = S5P_FIMV_CH_LAST_FRAME;
-        else
-                cmd = S5P_FIMV_CH_FRAME_START;
-        mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
-                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-
-        return 0;
-}
-
-static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
-{
-        unsigned long flags;
-        int new_ctx;
-        int cnt;
-
-        spin_lock_irqsave(&dev->condlock, flags);
-        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
-        cnt = 0;
-        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
-                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
-                if (++cnt > MFC_NUM_CONTEXTS) {
-                        /* No contexts to run */
-                        spin_unlock_irqrestore(&dev->condlock, flags);
-                        return -EAGAIN;
-                }
-        }
-        spin_unlock_irqrestore(&dev->condlock, flags);
-        return new_ctx;
-}
-
-static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        s5p_mfc_set_dec_stream_buffer(ctx, 0, 0, 0);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        s5p_mfc_decode_one_frame(ctx, MFC_DEC_RES_CHANGE);
-}
-
-static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_buf *temp_vb;
-        unsigned long flags;
-        unsigned int index;
-
-        spin_lock_irqsave(&dev->irqlock, flags);
-        /* Frames are being decoded */
-        if (list_empty(&ctx->src_queue)) {
-                mfc_debug(2, "No src buffers\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EAGAIN;
-        }
-        /* Get the next source buffer */
-        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-        temp_vb->flags |= MFC_BUF_FLAG_USED;
-        s5p_mfc_set_dec_stream_buffer(ctx,
-                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), ctx->consumed_stream,
-                                        temp_vb->b->v4l2_planes[0].bytesused);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        index = temp_vb->b->v4l2_buf.index;
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
-                last_frame = MFC_DEC_LAST_FRAME;
-                mfc_debug(2, "Setting ctx->state to FINISHING\n");
-                ctx->state = MFCINST_FINISHING;
-        }
-        s5p_mfc_decode_one_frame(ctx, last_frame);
-        return 0;
-}
-
-static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *dst_mb;
-        struct s5p_mfc_buf *src_mb;
-        unsigned long src_y_addr, src_c_addr, dst_addr;
-        unsigned int dst_size;
-
-        spin_lock_irqsave(&dev->irqlock, flags);
-        if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
-                mfc_debug(2, "no src buffers\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EAGAIN;
-        }
-        if (list_empty(&ctx->dst_queue)) {
-                mfc_debug(2, "no dst buffers\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EAGAIN;
-        }
-        if (list_empty(&ctx->src_queue)) {
-                /* send null frame */
-                s5p_mfc_set_enc_frame_buffer(ctx, dev->bank2, dev->bank2);
-                src_mb = NULL;
-        } else {
-                src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
-                                                                        list);
-                src_mb->flags |= MFC_BUF_FLAG_USED;
-                if (src_mb->b->v4l2_planes[0].bytesused == 0) {
-                        /* send null frame */
-                        s5p_mfc_set_enc_frame_buffer(ctx, dev->bank2,
-                                                                dev->bank2);
-                        ctx->state = MFCINST_FINISHING;
-                } else {
-                        src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
-                                                                        0);
-                        src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
-                                                                        1);
-                        s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr,
-                                                                src_c_addr);
-                        if (src_mb->flags & MFC_BUF_FLAG_EOS)
-                                ctx->state = MFCINST_FINISHING;
-                }
-        }
-        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
-        dst_mb->flags |= MFC_BUF_FLAG_USED;
-        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
-        dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        mfc_debug(2, "encoding buffer with index=%d state=%d",
-                        src_mb ? src_mb->b->v4l2_buf.index : -1, ctx->state);
-        s5p_mfc_encode_one_frame(ctx);
-        return 0;
-}
-
-static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *temp_vb;
-
-        /* Initializing decoding - parsing header */
-        spin_lock_irqsave(&dev->irqlock, flags);
-        mfc_debug(2, "Preparing to init decoding\n");
-        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-        s5p_mfc_set_dec_desc_buffer(ctx);
-        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
-        s5p_mfc_set_dec_stream_buffer(ctx,
-                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
-                                0, temp_vb->b->v4l2_planes[0].bytesused);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        s5p_mfc_init_decode(ctx);
-}
-
-static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *dst_mb;
-        unsigned long dst_addr;
-        unsigned int dst_size;
-
-        s5p_mfc_set_enc_ref_buffer(ctx);
-        spin_lock_irqsave(&dev->irqlock, flags);
-        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
-        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
-        dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        s5p_mfc_init_encode(ctx);
-}
-
-static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *temp_vb;
-        int ret;
-
-        /*
-         * Header was parsed now starting processing
-         * First set the output frame buffers
-         */
-        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
-                mfc_err("It seems that not all destionation buffers were "
-                        "mmaped\nMFC requires that all destination are mmaped "
-                        "before starting processing\n");
-                return -EAGAIN;
-        }
-        spin_lock_irqsave(&dev->irqlock, flags);
-        if (list_empty(&ctx->src_queue)) {
-                mfc_err("Header has been deallocated in the middle of"
-                        " initialization\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EIO;
-        }
-        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
-        s5p_mfc_set_dec_stream_buffer(ctx,
-                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
-                                0, temp_vb->b->v4l2_planes[0].bytesused);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        ret = s5p_mfc_set_dec_frame_buffer(ctx);
-        if (ret) {
-                mfc_err("Failed to alloc frame mem\n");
-                ctx->state = MFCINST_ERROR;
-        }
-        return ret;
-}
-
-/* Try running an operation on hardware */
-void s5p_mfc_try_run(struct s5p_mfc_dev *dev)
-{
-        struct s5p_mfc_ctx *ctx;
-        int new_ctx;
-        unsigned int ret = 0;
-
-        if (test_bit(0, &dev->enter_suspend)) {
-                mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
-                return;
-        }
-        /* Check whether hardware is not running */
-        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
-                /* This is perfectly ok, the scheduled ctx should wait */
-                mfc_debug(1, "Couldn't lock HW\n");
-                return;
-        }
-        /* Choose the context to run */
-        new_ctx = s5p_mfc_get_new_ctx(dev);
-        if (new_ctx < 0) {
-                /* No contexts to run */
-                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
-                        mfc_err("Failed to unlock hardware\n");
-                        return;
-                }
-                mfc_debug(1, "No ctx is scheduled to be run\n");
-                return;
-        }
-        ctx = dev->ctx[new_ctx];
-        /* Got context to run in ctx */
-        /*
-         * Last frame has already been sent to MFC.
-         * Now obtaining frames from MFC buffer
-         */
-        s5p_mfc_clock_on();
-        if (ctx->type == MFCINST_DECODER) {
-                s5p_mfc_set_dec_desc_buffer(ctx);
-                switch (ctx->state) {
-                case MFCINST_FINISHING:
-                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
-                        break;
-                case MFCINST_RUNNING:
-                        ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
-                        break;
-                case MFCINST_INIT:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_open_inst_cmd(ctx);
-                        break;
-                case MFCINST_RETURN_INST:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_close_inst_cmd(ctx);
-                        break;
-                case MFCINST_GOT_INST:
-                        s5p_mfc_run_init_dec(ctx);
-                        break;
-                case MFCINST_HEAD_PARSED:
-                        ret = s5p_mfc_run_init_dec_buffers(ctx);
-                        mfc_debug(1, "head parsed\n");
-                        break;
-                case MFCINST_RES_CHANGE_INIT:
-                        s5p_mfc_run_res_change(ctx);
-                        break;
-                case MFCINST_RES_CHANGE_FLUSH:
-                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
-                        break;
-                case MFCINST_RES_CHANGE_END:
-                        mfc_debug(2, "Finished remaining frames after resolution change\n");
-                        ctx->capture_state = QUEUE_FREE;
-                        mfc_debug(2, "Will re-init the codec\n");
-                        s5p_mfc_run_init_dec(ctx);
-                        break;
-                default:
-                        ret = -EAGAIN;
-                }
-        } else if (ctx->type == MFCINST_ENCODER) {
-                switch (ctx->state) {
-                case MFCINST_FINISHING:
-                case MFCINST_RUNNING:
-                        ret = s5p_mfc_run_enc_frame(ctx);
-                        break;
-                case MFCINST_INIT:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_open_inst_cmd(ctx);
-                        break;
-                case MFCINST_RETURN_INST:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_close_inst_cmd(ctx);
-                        break;
-                case MFCINST_GOT_INST:
-                        s5p_mfc_run_init_enc(ctx);
-                        break;
-                default:
-                        ret = -EAGAIN;
-                }
-        } else {
-                mfc_err("Invalid context type: %d\n", ctx->type);
-                ret = -EAGAIN;
-        }
-
-        if (ret) {
-                /* Free hardware lock */
-                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
-                        mfc_err("Failed to unlock hardware\n");
-
-                /* This is in deed imporant, as no operation has been
-                 * scheduled, reduce the clock count as no one will
-                 * ever do this, because no interrupt related to this try_run
-                 * will ever come from hardware. */
-                s5p_mfc_clock_off();
-        }
-}
-
-
-void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq)
-{
-        struct s5p_mfc_buf *b;
-        int i;
-
-        while (!list_empty(lh)) {
-                b = list_entry(lh->next, struct s5p_mfc_buf, list);
-                for (i = 0; i < b->b->num_planes; i++)
-                        vb2_set_plane_payload(b->b, i, 0);
-                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
-                list_del(&b->list);
-        }
-}
-
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index 2ad3def052f8..420abecafec0 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -1,10 +1,10 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_opr.h
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
  *
  * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
  * Contains declarations of hw related functions.
  *
- * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * Kamil Debski, Copyright (C) 2012 Samsung Electronics Co., Ltd.
  * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,77 +17,68 @@
 
 #include "s5p_mfc_common.h"
 
-int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_init_encode(struct s5p_mfc_ctx *mfc_ctx);
+struct s5p_mfc_hw_ops {
+        int (*alloc_dec_temp_buffers)(struct s5p_mfc_ctx *ctx);
+        void (*release_dec_desc_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*alloc_codec_buffers)(struct s5p_mfc_ctx *ctx);
+        void (*release_codec_buffers)(struct s5p_mfc_ctx *ctx);
+        int (*alloc_instance_buffer)(struct s5p_mfc_ctx *ctx);
+        void (*release_instance_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*alloc_dev_context_buffer)(struct s5p_mfc_dev *dev);
+        void (*release_dev_context_buffer)(struct s5p_mfc_dev *dev);
+        void (*dec_calc_dpb_size)(struct s5p_mfc_ctx *ctx);
+        void (*enc_calc_src_size)(struct s5p_mfc_ctx *ctx);
+        int (*set_dec_stream_buffer)(struct s5p_mfc_ctx *ctx,
+                        int buf_addr, unsigned int start_num_byte,
+                        unsigned int buf_size);
+        int (*set_dec_frame_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*set_enc_stream_buffer)(struct s5p_mfc_ctx *ctx,
+                        unsigned long addr, unsigned int size);
+        void (*set_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+                        unsigned long y_addr, unsigned long c_addr);
+        void (*get_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+                        unsigned long *y_addr, unsigned long *c_addr);
+        int (*set_enc_ref_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*init_decode)(struct s5p_mfc_ctx *ctx);
+        int (*init_encode)(struct s5p_mfc_ctx *ctx);
+        int (*encode_one_frame)(struct s5p_mfc_ctx *ctx);
+        void (*try_run)(struct s5p_mfc_dev *dev);
+        void (*cleanup_queue)(struct list_head *lh,
+                        struct vb2_queue *vq);
+        void (*clear_int_flags)(struct s5p_mfc_dev *dev);
+        void (*write_info)(struct s5p_mfc_ctx *ctx, unsigned int data,
+                        unsigned int ofs);
+        unsigned int (*read_info)(struct s5p_mfc_ctx *ctx,
+                        unsigned int ofs);
+        int (*get_dspl_y_adr)(struct s5p_mfc_dev *dev);
+        int (*get_dec_y_adr)(struct s5p_mfc_dev *dev);
+        int (*get_dspl_status)(struct s5p_mfc_dev *dev);
+        int (*get_dec_status)(struct s5p_mfc_dev *dev);
+        int (*get_dec_frame_type)(struct s5p_mfc_dev *dev);
+        int (*get_disp_frame_type)(struct s5p_mfc_ctx *ctx);
+        int (*get_consumed_stream)(struct s5p_mfc_dev *dev);
+        int (*get_int_reason)(struct s5p_mfc_dev *dev);
+        int (*get_int_err)(struct s5p_mfc_dev *dev);
+        int (*err_dec)(unsigned int err);
+        int (*err_dspl)(unsigned int err);
+        int (*get_img_width)(struct s5p_mfc_dev *dev);
+        int (*get_img_height)(struct s5p_mfc_dev *dev);
+        int (*get_dpb_count)(struct s5p_mfc_dev *dev);
+        int (*get_mv_count)(struct s5p_mfc_dev *dev);
+        int (*get_inst_no)(struct s5p_mfc_dev *dev);
+        int (*get_enc_strm_size)(struct s5p_mfc_dev *dev);
+        int (*get_enc_slice_type)(struct s5p_mfc_dev *dev);
+        int (*get_enc_dpb_count)(struct s5p_mfc_dev *dev);
+        int (*get_enc_pic_count)(struct s5p_mfc_dev *dev);
+        int (*get_sei_avail_status)(struct s5p_mfc_ctx *ctx);
+        int (*get_mvc_num_views)(struct s5p_mfc_dev *dev);
+        int (*get_mvc_view_id)(struct s5p_mfc_dev *dev);
+        unsigned int (*get_pic_type_top)(struct s5p_mfc_ctx *ctx);
+        unsigned int (*get_pic_type_bot)(struct s5p_mfc_ctx *ctx);
+        unsigned int (*get_crop_info_h)(struct s5p_mfc_ctx *ctx);
+        unsigned int (*get_crop_info_v)(struct s5p_mfc_ctx *ctx);
+};
 
-/* Decoding functions */
-int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
-                                                  unsigned int start_num_byte,
-                                                  unsigned int buf_size);
-
-/* Encoding functions */
-void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long y_addr, unsigned long c_addr);
-int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long addr, unsigned int size);
-void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long *y_addr, unsigned long *c_addr);
-int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *mfc_ctx);
-
-int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
-                                        enum s5p_mfc_decode_arg last_frame);
-int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *mfc_ctx);
-
-/* Memory allocation */
-int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
-
-int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx);
-
-int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx);
-
-void s5p_mfc_try_run(struct s5p_mfc_dev *dev);
-void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq);
-
-#define s5p_mfc_get_dspl_y_adr()        (readl(dev->regs_base + \
-                                        S5P_FIMV_SI_DISPLAY_Y_ADR) << \
-                                        MFC_OFFSET_SHIFT)
-#define s5p_mfc_get_dec_y_adr()         (readl(dev->regs_base + \
-                                        S5P_FIMV_SI_DECODE_Y_ADR) << \
-                                        MFC_OFFSET_SHIFT)
-#define s5p_mfc_get_dspl_status()       readl(dev->regs_base + \
-                                                S5P_FIMV_SI_DISPLAY_STATUS)
-#define s5p_mfc_get_dec_status()        readl(dev->regs_base + \
-                                                S5P_FIMV_SI_DECODE_STATUS)
-#define s5p_mfc_get_frame_type()        (readl(dev->regs_base + \
-                                                S5P_FIMV_DECODE_FRAME_TYPE) \
-                                        & S5P_FIMV_DECODE_FRAME_MASK)
-#define s5p_mfc_get_consumed_stream()   readl(dev->regs_base + \
-                                                S5P_FIMV_SI_CONSUMED_BYTES)
-#define s5p_mfc_get_int_reason()        (readl(dev->regs_base + \
-                                        S5P_FIMV_RISC2HOST_CMD) & \
-                                        S5P_FIMV_RISC2HOST_CMD_MASK)
-#define s5p_mfc_get_int_err()           readl(dev->regs_base + \
-                                                S5P_FIMV_RISC2HOST_ARG2)
-#define s5p_mfc_err_dec(x)              (((x) & S5P_FIMV_ERR_DEC_MASK) >> \
-                                                        S5P_FIMV_ERR_DEC_SHIFT)
-#define s5p_mfc_err_dspl(x)             (((x) & S5P_FIMV_ERR_DSPL_MASK) >> \
-                                                        S5P_FIMV_ERR_DSPL_SHIFT)
-#define s5p_mfc_get_img_width()         readl(dev->regs_base + \
-                                                S5P_FIMV_SI_HRESOL)
-#define s5p_mfc_get_img_height()        readl(dev->regs_base + \
-                                                S5P_FIMV_SI_VRESOL)
-#define s5p_mfc_get_dpb_count()         readl(dev->regs_base + \
-                                                S5P_FIMV_SI_BUF_NUMBER)
-#define s5p_mfc_get_inst_no()           readl(dev->regs_base + \
-                                                S5P_FIMV_RISC2HOST_ARG1)
-#define s5p_mfc_get_enc_strm_size()     readl(dev->regs_base + \
-                                                S5P_FIMV_ENC_SI_STRM_SIZE)
-#define s5p_mfc_get_enc_slice_type()    readl(dev->regs_base + \
-                                                S5P_FIMV_ENC_SI_SLICE_TYPE)
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev);
 
 #endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
new file mode 100644
index 000000000000..bf7d010a4107
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -0,0 +1,1794 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v5.h"
+#include <asm/cacheflush.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#define OFFSETA(x)              (((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
+#define OFFSETB(x)              (((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+        ctx->dsc.alloc = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX],
+                        buf_size->dsc);
+        if (IS_ERR_VALUE((int)ctx->dsc.alloc)) {
+                ctx->dsc.alloc = NULL;
+                mfc_err("Allocating DESC buffer failed\n");
+                return -ENOMEM;
+        }
+        ctx->dsc.dma = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->dsc.alloc);
+        BUG_ON(ctx->dsc.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        ctx->dsc.virt = vb2_dma_contig_memops.vaddr(ctx->dsc.alloc);
+        if (ctx->dsc.virt == NULL) {
+                vb2_dma_contig_memops.put(ctx->dsc.alloc);
+                ctx->dsc.dma = 0;
+                ctx->dsc.alloc = NULL;
+                mfc_err("Remapping DESC buffer failed\n");
+                return -ENOMEM;
+        }
+        memset(ctx->dsc.virt, 0, buf_size->dsc);
+        wmb();
+        return 0;
+}
+
+/* Release temporary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->dsc.dma) {
+                vb2_dma_contig_memops.put(ctx->dsc.alloc);
+                ctx->dsc.alloc = NULL;
+                ctx->dsc.dma = 0;
+        }
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int enc_ref_y_size = 0;
+        unsigned int enc_ref_c_size = 0;
+        unsigned int guard_width, guard_height;
+
+        if (ctx->type == MFCINST_DECODER) {
+                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+        } else if (ctx->type == MFCINST_ENCODER) {
+                enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+
+                if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+                        enc_ref_c_size = ALIGN(ctx->img_width,
+                                                S5P_FIMV_NV12MT_HALIGN)
+                                                * ALIGN(ctx->img_height >> 1,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                        enc_ref_c_size = ALIGN(enc_ref_c_size,
+                                                        S5P_FIMV_NV12MT_SALIGN);
+                } else {
+                        guard_width = ALIGN(ctx->img_width + 16,
+                                                        S5P_FIMV_NV12MT_HALIGN);
+                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                        S5P_FIMV_NV12MT_VALIGN);
+                        enc_ref_c_size = ALIGN(guard_width * guard_height,
+                                               S5P_FIMV_NV12MT_SALIGN);
+                }
+                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+                          enc_ref_y_size, enc_ref_c_size);
+        } else {
+                return -EINVAL;
+        }
+        /* Codecs have different memory requirements */
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
+                                        S5P_FIMV_DEC_VERT_NB_MV_SIZE,
+                                        S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
+                                     S5P_FIMV_DEC_UPNB_MV_SIZE +
+                                     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                                     S5P_FIMV_DEC_STX_PARSER_SIZE +
+                                     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
+                                     S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+        case S5P_MFC_CODEC_VC1_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+                             S5P_FIMV_DEC_UPNB_MV_SIZE +
+                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                             S5P_FIMV_DEC_NB_DCAC_SIZE +
+                             3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
+                             S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                ctx->bank1_size = 0;
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+                             S5P_FIMV_DEC_UPNB_MV_SIZE +
+                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                             S5P_FIMV_DEC_NB_DCAC_SIZE,
+                             S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_COLFLG_SIZE +
+                                   S5P_FIMV_ENC_INTRAMD_SIZE +
+                                   S5P_FIMV_ENC_NBORINFO_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4) +
+                                   S5P_FIMV_ENC_INTRAPRED_SIZE;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_COLFLG_SIZE +
+                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4);
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4);
+                break;
+        default:
+                break;
+        }
+        /* Allocate only if memory from bank 1 is necessary */
+        if (ctx->bank1_size > 0) {
+                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+                if (IS_ERR(ctx->bank1_buf)) {
+                        ctx->bank1_buf = NULL;
+                        printk(KERN_ERR
+                               "Buf alloc for decoding failed (port A)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank1_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        }
+        /* Allocate only if memory from bank 2 is necessary */
+        if (ctx->bank2_size > 0) {
+                ctx->bank2_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
+                if (IS_ERR(ctx->bank2_buf)) {
+                        ctx->bank2_buf = NULL;
+                        mfc_err("Buf alloc for decoding failed (port B)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank2_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
+                BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
+        }
+        return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->bank1_buf) {
+                vb2_dma_contig_memops.put(ctx->bank1_buf);
+                ctx->bank1_buf = NULL;
+                ctx->bank1_phys = 0;
+                ctx->bank1_size = 0;
+        }
+        if (ctx->bank2_buf) {
+                vb2_dma_contig_memops.put(ctx->bank2_buf);
+                ctx->bank2_buf = NULL;
+                ctx->bank2_phys = 0;
+                ctx->bank2_size = 0;
+        }
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                ctx->ctx.size = buf_size->h264_ctx;
+        else
+                ctx->ctx.size = buf_size->non_h264_ctx;
+        ctx->ctx.alloc = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.size);
+        if (IS_ERR(ctx->ctx.alloc)) {
+                mfc_err("Allocating context buffer failed\n");
+                ctx->ctx.alloc = NULL;
+                return -ENOMEM;
+        }
+        ctx->ctx.dma = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.alloc);
+        BUG_ON(ctx->ctx.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        ctx->ctx.ofs = OFFSETA(ctx->ctx.dma);
+        ctx->ctx.virt = vb2_dma_contig_memops.vaddr(ctx->ctx.alloc);
+        if (!ctx->ctx.virt) {
+                mfc_err("Remapping instance buffer failed\n");
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.ofs = 0;
+                ctx->ctx.dma = 0;
+                return -ENOMEM;
+        }
+        /* Zero content of the allocated memory */
+        memset(ctx->ctx.virt, 0, ctx->ctx.size);
+        wmb();
+
+        /* Initialize shared memory */
+        ctx->shm.alloc = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], buf_size->shm);
+        if (IS_ERR(ctx->shm.alloc)) {
+                mfc_err("failed to allocate shared memory\n");
+                return PTR_ERR(ctx->shm.alloc);
+        }
+        /* shared memory offset only keeps the offset from base (port a) */
+        ctx->shm.ofs = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->shm.alloc)
+                                                                - dev->bank1;
+        BUG_ON(ctx->shm.ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+
+        ctx->shm.virt = vb2_dma_contig_memops.vaddr(ctx->shm.alloc);
+        if (!ctx->shm.virt) {
+                vb2_dma_contig_memops.put(ctx->shm.alloc);
+                ctx->shm.alloc = NULL;
+                ctx->shm.ofs = 0;
+                mfc_err("failed to virt addr of shared memory\n");
+                return -ENOMEM;
+        }
+        memset((void *)ctx->shm.virt, 0, buf_size->shm);
+        wmb();
+        return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->ctx.alloc) {
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.ofs = 0;
+                ctx->ctx.virt = NULL;
+                ctx->ctx.dma = 0;
+        }
+        if (ctx->shm.alloc) {
+                vb2_dma_contig_memops.put(ctx->shm.alloc);
+                ctx->shm.alloc = NULL;
+                ctx->shm.ofs = 0;
+                ctx->shm.virt = NULL;
+        }
+}
+
+int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+
+        return 0;
+}
+
+void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+}
+
+static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
+                        unsigned int ofs)
+{
+        writel(data, (ctx->shm.virt + ofs));
+        wmb();
+}
+
+static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
+                                unsigned int ofs)
+{
+        rmb();
+        return readl(ctx->shm.virt + ofs);
+}
+
+void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int guard_width, guard_height;
+
+        ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+        ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+        mfc_debug(2,
+                "SEQ Done: Movie dimensions %dx%d, buffer dimensions: %dx%d\n",
+                ctx->img_width, ctx->img_height, ctx->buf_width,
+                ctx->buf_height);
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+                ctx->luma_size = ALIGN(ctx->buf_width * ctx->buf_height,
+                                S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->chroma_size = ALIGN(ctx->buf_width *
+                                ALIGN((ctx->img_height >> 1),
+                                        S5P_FIMV_NV12MT_VALIGN),
+                                S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->mv_size = ALIGN(ctx->buf_width *
+                                ALIGN((ctx->buf_height >> 2),
+                                        S5P_FIMV_NV12MT_VALIGN),
+                                S5P_FIMV_DEC_BUF_ALIGN);
+        } else {
+                guard_width =
+                        ALIGN(ctx->img_width + 24, S5P_FIMV_NV12MT_HALIGN);
+                guard_height =
+                        ALIGN(ctx->img_height + 16, S5P_FIMV_NV12MT_VALIGN);
+                ctx->luma_size = ALIGN(guard_width * guard_height,
+                                S5P_FIMV_DEC_BUF_ALIGN);
+
+                guard_width =
+                        ALIGN(ctx->img_width + 16, S5P_FIMV_NV12MT_HALIGN);
+                guard_height =
+                        ALIGN((ctx->img_height >> 1) + 4,
+                                        S5P_FIMV_NV12MT_VALIGN);
+                ctx->chroma_size = ALIGN(guard_width * guard_height,
+                                S5P_FIMV_DEC_BUF_ALIGN);
+
+                ctx->mv_size = 0;
+        }
+}
+
+void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+                ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN);
+
+                ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12M_LVALIGN);
+                ctx->chroma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12M_CVALIGN);
+
+                ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12M_SALIGN);
+                ctx->chroma_size =
+                        ALIGN(ctx->chroma_size, S5P_FIMV_NV12M_SALIGN);
+        } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
+                ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+
+                ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+                ctx->chroma_size =
+                        ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+
+                ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12MT_SALIGN);
+                ctx->chroma_size =
+                        ALIGN(ctx->chroma_size, S5P_FIMV_NV12MT_SALIGN);
+        }
+}
+
+/* Set registers for decoding temporary buffers */
+static void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+        mfc_write(dev, OFFSETA(ctx->dsc.dma), S5P_FIMV_SI_CH0_DESC_ADR);
+        mfc_write(dev, buf_size->dsc, S5P_FIMV_SI_CH0_DESC_SIZE);
+}
+
+/* Set registers for shared buffer */
+static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        mfc_write(dev, ctx->shm.ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx, int buf_addr,
+                  unsigned int start_num_byte, unsigned int buf_size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
+        mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
+        mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
+        s5p_mfc_write_info_v5(ctx, start_num_byte, START_BYTE_NUM);
+        return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int frame_size, i;
+        unsigned int frame_size_ch, frame_size_mv;
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+        size_t buf_addr1, buf_addr2;
+        int buf_size1, buf_size2;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+        buf_addr2 = ctx->bank2_phys;
+        buf_size2 = ctx->bank2_size;
+        dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+                                                ~S5P_FIMV_DPB_COUNT_MASK;
+        mfc_write(dev, ctx->total_dpb_count | dpb,
+                                                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+        s5p_mfc_set_shared_buffer(ctx);
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_H264_VERT_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
+                buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                break;
+        case S5P_MFC_CODEC_VC1_DEC:
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                break;
+        default:
+                mfc_err("Unknown codec for decoding (%x)\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+                break;
+        }
+        frame_size = ctx->luma_size;
+        frame_size_ch = ctx->chroma_size;
+        frame_size_mv = ctx->mv_size;
+        mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
+                                                                frame_size_mv);
+        for (i = 0; i < ctx->total_dpb_count; i++) {
+                /* Bank2 */
+                mfc_debug(2, "Luma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.luma);
+                mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
+                                                S5P_FIMV_DEC_LUMA_ADR + i * 4);
+                mfc_debug(2, "\tChroma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.chroma);
+                mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
+                                               S5P_FIMV_DEC_CHROMA_ADR + i * 4);
+                if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+                        mfc_debug(2, "\tBuf2: %x, size: %d\n",
+                                                        buf_addr2, buf_size2);
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                                S5P_FIMV_H264_MV_ADR + i * 4);
+                        buf_addr2 += frame_size_mv;
+                        buf_size2 -= frame_size_mv;
+                }
+        }
+        mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
+        mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
+                        buf_size1,  buf_size2, ctx->total_dpb_count);
+        if (buf_size1 < 0 || buf_size2 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated\n");
+                return -ENOMEM;
+        }
+        s5p_mfc_write_info_v5(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
+        s5p_mfc_write_info_v5(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC)
+                s5p_mfc_write_info_v5(ctx, frame_size_mv, ALLOC_MV_SIZE);
+        mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
+                                        << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+                                                S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+                unsigned long addr, unsigned int size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
+        mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
+        return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+                unsigned long y_addr, unsigned long c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
+        mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
+}
+
+void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+                unsigned long *y_addr, unsigned long *c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        *y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
+                                                        << MFC_OFFSET_SHIFT);
+        *c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
+                                                        << MFC_OFFSET_SHIFT);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1, buf_addr2;
+        size_t buf_size1, buf_size2;
+        unsigned int enc_ref_y_size, enc_ref_c_size;
+        unsigned int guard_width, guard_height;
+        int i;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+        buf_addr2 = ctx->bank2_phys;
+        buf_size2 = ctx->bank2_size;
+        enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+        enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+                enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
+        } else {
+                guard_width = ALIGN(ctx->img_width + 16,
+                                                S5P_FIMV_NV12MT_HALIGN);
+                guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_c_size = ALIGN(guard_width * guard_height,
+                                       S5P_FIMV_NV12MT_SALIGN);
+        }
+        mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_COZERO_FLAG_ADR);
+                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_UP_INTRA_MD_ADR);
+                buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
+                mfc_write(dev, OFFSETB(buf_addr2),
+                                        S5P_FIMV_H264_UP_INTRA_PRED_ADR);
+                buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
+                buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_NBOR_INFO_ADR);
+                buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
+                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_MPEG4_ACDC_COEF_ADR);
+                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
+                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        default:
+                mfc_err("Unknown codec set for encoding: %d\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        unsigned int reg;
+        unsigned int shm;
+
+        /* width */
+        mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
+        /* height */
+        mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
+        /* pictype : enable, IDR period */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        reg |= (1 << 18);
+        reg &= ~(0xFFFF);
+        reg |= p->gop_size;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
+        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
+        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
+        } else {
+                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
+                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
+        }
+        /* cyclic intra refresh */
+        mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+        /* padding control & value */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
+        if (p->pad) {
+                /** enable */
+                reg |= (1 << 31);
+                /** cr value */
+                reg &= ~(0xFF << 16);
+                reg |= (p->pad_cr << 16);
+                /** cb value */
+                reg &= ~(0xFF << 8);
+                reg |= (p->pad_cb << 8);
+                /** y value */
+                reg &= ~(0xFF);
+                reg |= (p->pad_luma);
+        } else {
+                /** disable & all value clear */
+                reg = 0;
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /** frame-level rate control */
+        reg &= ~(0x1 << 9);
+        reg |= (p->rc_frame << 9);
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* bit rate */
+        if (p->rc_frame)
+                mfc_write(dev, p->rc_bitrate,
+                        S5P_FIMV_ENC_RC_BIT_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
+        /* reaction coefficient */
+        if (p->rc_frame)
+                mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* seq header ctrl */
+        shm &= ~(0x1 << 3);
+        shm |= (p->seq_hdr_mode << 3);
+        /* frame skip mode */
+        shm &= ~(0x3 << 1);
+        shm |= (p->frame_skip_mode << 1);
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        /* fixed target bit */
+        s5p_mfc_write_info_v5(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
+        unsigned int reg;
+        unsigned int shm;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* pictype : number of B */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* num_b_frame - 0 ~ 2 */
+        reg &= ~(0x3 << 16);
+        reg |= (p->num_b_frame << 16);
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* profile & level */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+        /* level */
+        reg &= ~(0xFF << 8);
+        reg |= (p_264->level << 8);
+        /* profile - 0 ~ 2 */
+        reg &= ~(0x3F);
+        reg |= p_264->profile;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+        /* interlace  */
+        mfc_write(dev, p_264->interlace, S5P_FIMV_ENC_PIC_STRUCT);
+        /* height */
+        if (p_264->interlace)
+                mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
+        /* loopfilter ctrl */
+        mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
+        /* loopfilter alpha offset */
+        if (p_264->loop_filter_alpha < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_264->loop_filter_alpha) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_264->loop_filter_alpha & 0xF);
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
+        /* loopfilter beta offset */
+        if (p_264->loop_filter_beta < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_264->loop_filter_beta) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_264->loop_filter_beta & 0xF);
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
+        /* entropy coding mode */
+        if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
+                mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+        /* number of ref. picture */
+        reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
+        /* num of ref. pictures of P */
+        reg &= ~(0x3 << 5);
+        reg |= (p_264->num_ref_pic_4p << 5);
+        /* max number of ref. pictures */
+        reg &= ~(0x1F);
+        reg |= p_264->max_ref_pic;
+        mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
+        /* 8x8 transform enable */
+        mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= (p->rc_mb << 8);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_264->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_denom)
+                mfc_write(dev, p->rc_framerate_num * 1000
+                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_264->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_264->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* macroblock adaptive scaling features */
+        if (p->rc_mb) {
+                reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
+                /* dark region */
+                reg &= ~(0x1 << 3);
+                reg |= (p_264->rc_mb_dark << 3);
+                /* smooth region */
+                reg &= ~(0x1 << 2);
+                reg |= (p_264->rc_mb_smooth << 2);
+                /* static region */
+                reg &= ~(0x1 << 1);
+                reg |= (p_264->rc_mb_static << 1);
+                /* high activity region */
+                reg &= ~(0x1);
+                reg |= p_264->rc_mb_activity;
+                mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
+        }
+        if (!p->rc_frame && !p->rc_mb) {
+                shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
+                shm |= (p_264->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+        }
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* AR VUI control */
+        shm &= ~(0x1 << 15);
+        shm |= (p_264->vui_sar << 1);
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        if (p_264->vui_sar) {
+                /* aspect ration IDC */
+                shm = s5p_mfc_read_info_v5(ctx, SAMPLE_ASPECT_RATIO_IDC);
+                shm &= ~(0xFF);
+                shm |= p_264->vui_sar_idc;
+                s5p_mfc_write_info_v5(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
+                if (p_264->vui_sar_idc == 0xFF) {
+                        /* sample  AR info */
+                        shm = s5p_mfc_read_info_v5(ctx, EXTENDED_SAR);
+                        shm &= ~(0xFFFFFFFF);
+                        shm |= p_264->vui_ext_sar_width << 16;
+                        shm |= p_264->vui_ext_sar_height;
+                        s5p_mfc_write_info_v5(ctx, shm, EXTENDED_SAR);
+                }
+        }
+        /* intra picture period for H.264 */
+        shm = s5p_mfc_read_info_v5(ctx, H264_I_PERIOD);
+        /* control */
+        shm &= ~(0x1 << 16);
+        shm |= (p_264->open_gop << 16);
+        /* value */
+        if (p_264->open_gop) {
+                shm &= ~(0xFFFF);
+                shm |= p_264->open_gop_size;
+        }
+        s5p_mfc_write_info_v5(ctx, shm, H264_I_PERIOD);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p_264->cpb_size << 16);
+        }
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+        unsigned int reg;
+        unsigned int shm;
+        unsigned int framerate;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* pictype : number of B */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* num_b_frame - 0 ~ 2 */
+        reg &= ~(0x3 << 16);
+        reg |= (p->num_b_frame << 16);
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* profile & level */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+        /* level */
+        reg &= ~(0xFF << 8);
+        reg |= (p_mpeg4->level << 8);
+        /* profile - 0 ~ 2 */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->profile;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+        /* quarter_pixel */
+        mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
+        /* qp */
+        if (!p->rc_frame) {
+                shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
+                shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+        }
+        /* frame rate */
+        if (p->rc_frame) {
+                if (p->rc_framerate_denom > 0) {
+                        framerate = p->rc_framerate_num * 1000 /
+                                                p->rc_framerate_denom;
+                        mfc_write(dev, framerate,
+                                S5P_FIMV_ENC_RC_FRAME_RATE);
+                        shm = s5p_mfc_read_info_v5(ctx, RC_VOP_TIMING);
+                        shm &= ~(0xFFFFFFFF);
+                        shm |= (1 << 31);
+                        shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
+                        shm |= (p->rc_framerate_denom & 0xFFFF);
+                        s5p_mfc_write_info_v5(ctx, shm, RC_VOP_TIMING);
+                }
+        } else {
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        }
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_mpeg4->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p->vbv_size << 16);
+        }
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+        unsigned int reg;
+        unsigned int shm;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* qp */
+        if (!p->rc_frame) {
+                shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= (p_h263->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+        }
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_denom)
+                mfc_write(dev, p->rc_framerate_num * 1000
+                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_h263->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p->vbv_size << 16);
+        }
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_set_shared_buffer(ctx);
+        /* Setup loop filter, for decoding this is only valid for MPEG4 */
+        if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC)
+                mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
+        mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
+                S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
+                S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
+                S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
+                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+        mfc_write(dev,
+        ((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+
+        if (flush)
+                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
+                        S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+        else
+                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+                        ~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+        mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
+                                        enum s5p_mfc_decode_arg last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
+        s5p_mfc_set_shared_buffer(ctx);
+        s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
+        /* Issue different commands to instance basing on whether it
+         * is the last frame or not. */
+        switch (last_frame) {
+        case MFC_DEC_FRAME:
+                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
+                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        case MFC_DEC_LAST_FRAME:
+                mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
+                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        case MFC_DEC_RES_CHANGE:
+                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
+                S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+                S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        }
+        mfc_debug(2, "Decoding a usual frame\n");
+        return 0;
+}
+
+int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                s5p_mfc_set_enc_params_h264(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+                s5p_mfc_set_enc_params_mpeg4(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+                s5p_mfc_set_enc_params_h263(ctx);
+        else {
+                mfc_err("Unknown codec for encoding (%x)\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+        s5p_mfc_set_shared_buffer(ctx);
+        mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
+                (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int cmd;
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+        s5p_mfc_set_shared_buffer(ctx);
+
+        if (ctx->state == MFCINST_FINISHING)
+                cmd = S5P_FIMV_CH_LAST_FRAME;
+        else
+                cmd = S5P_FIMV_CH_FRAME_START;
+        mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+
+        return 0;
+}
+
+static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+        unsigned long flags;
+        int new_ctx;
+        int cnt;
+
+        spin_lock_irqsave(&dev->condlock, flags);
+        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+        cnt = 0;
+        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+                if (++cnt > MFC_NUM_CONTEXTS) {
+                        /* No contexts to run */
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                        return -EAGAIN;
+                }
+        }
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        return new_ctx;
+}
+
+static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_set_dec_stream_buffer_v5(ctx, 0, 0, 0);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_decode_one_frame_v5(ctx, MFC_DEC_RES_CHANGE);
+}
+
+static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->flags |= MFC_BUF_FLAG_USED;
+        s5p_mfc_set_dec_stream_buffer_v5(ctx,
+                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                ctx->consumed_stream, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        index = temp_vb->b->v4l2_buf.index;
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+                last_frame = MFC_DEC_LAST_FRAME;
+                mfc_debug(2, "Setting ctx->state to FINISHING\n");
+                ctx->state = MFCINST_FINISHING;
+        }
+        s5p_mfc_decode_one_frame_v5(ctx, last_frame);
+        return 0;
+}
+
+static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        struct s5p_mfc_buf *src_mb;
+        unsigned long src_y_addr, src_c_addr, dst_addr;
+        unsigned int dst_size;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
+                mfc_debug(2, "no src buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        if (list_empty(&ctx->dst_queue)) {
+                mfc_debug(2, "no dst buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        if (list_empty(&ctx->src_queue)) {
+                /* send null frame */
+                s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2, dev->bank2);
+                src_mb = NULL;
+        } else {
+                src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+                                                                        list);
+                src_mb->flags |= MFC_BUF_FLAG_USED;
+                if (src_mb->b->v4l2_planes[0].bytesused == 0) {
+                        /* send null frame */
+                        s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2,
+                                                                dev->bank2);
+                        ctx->state = MFCINST_FINISHING;
+                } else {
+                        src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
+                                                                        0);
+                        src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
+                                                                        1);
+                        s5p_mfc_set_enc_frame_buffer_v5(ctx, src_y_addr,
+                                                                src_c_addr);
+                        if (src_mb->flags & MFC_BUF_FLAG_EOS)
+                                ctx->state = MFCINST_FINISHING;
+                }
+        }
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_mb->flags |= MFC_BUF_FLAG_USED;
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        mfc_debug(2, "encoding buffer with index=%d state=%d",
+                        src_mb ? src_mb->b->v4l2_buf.index : -1, ctx->state);
+        s5p_mfc_encode_one_frame_v5(ctx);
+        return 0;
+}
+
+static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+
+        /* Initializing decoding - parsing header */
+        spin_lock_irqsave(&dev->irqlock, flags);
+        mfc_debug(2, "Preparing to init decoding\n");
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        s5p_mfc_set_dec_desc_buffer(ctx);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer_v5(ctx,
+                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                                0, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_decode_v5(ctx);
+}
+
+static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long dst_addr;
+        unsigned int dst_size;
+
+        s5p_mfc_set_enc_ref_buffer_v5(ctx);
+        spin_lock_irqsave(&dev->irqlock, flags);
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_encode_v5(ctx);
+}
+
+static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+        int ret;
+
+        /*
+         * Header was parsed now starting processing
+         * First set the output frame buffers
+         */
+        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+                mfc_err("It seems that not all destionation buffers were "
+                        "mmaped\nMFC requires that all destination are mmaped "
+                        "before starting processing\n");
+                return -EAGAIN;
+        }
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (list_empty(&ctx->src_queue)) {
+                mfc_err("Header has been deallocated in the middle of"
+                        " initialization\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EIO;
+        }
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer_v5(ctx,
+                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                                0, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_dec_frame_buffer_v5(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_ctx *ctx;
+        int new_ctx;
+        unsigned int ret = 0;
+
+        if (test_bit(0, &dev->enter_suspend)) {
+                mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
+                return;
+        }
+        /* Check whether hardware is not running */
+        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+                /* This is perfectly ok, the scheduled ctx should wait */
+                mfc_debug(1, "Couldn't lock HW\n");
+                return;
+        }
+        /* Choose the context to run */
+        new_ctx = s5p_mfc_get_new_ctx(dev);
+        if (new_ctx < 0) {
+                /* No contexts to run */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+                        mfc_err("Failed to unlock hardware\n");
+                        return;
+                }
+                mfc_debug(1, "No ctx is scheduled to be run\n");
+                return;
+        }
+        ctx = dev->ctx[new_ctx];
+        /* Got context to run in ctx */
+        /*
+         * Last frame has already been sent to MFC.
+         * Now obtaining frames from MFC buffer
+         */
+        s5p_mfc_clock_on();
+        if (ctx->type == MFCINST_DECODER) {
+                s5p_mfc_set_dec_desc_buffer(ctx);
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
+                        break;
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED:
+                        ret = s5p_mfc_run_init_dec_buffers(ctx);
+                        mfc_debug(1, "head parsed\n");
+                        break;
+                case MFCINST_RES_CHANGE_INIT:
+                        s5p_mfc_run_res_change(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_FLUSH:
+                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+                        break;
+                case MFCINST_RES_CHANGE_END:
+                        mfc_debug(2, "Finished remaining frames after resolution change\n");
+                        ctx->capture_state = QUEUE_FREE;
+                        mfc_debug(2, "Will re-init the codec\n");
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else if (ctx->type == MFCINST_ENCODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_enc_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_enc(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else {
+                mfc_err("Invalid context type: %d\n", ctx->type);
+                ret = -EAGAIN;
+        }
+
+        if (ret) {
+                /* Free hardware lock */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        mfc_err("Failed to unlock hardware\n");
+
+                /* This is in deed imporant, as no operation has been
+                 * scheduled, reduce the clock count as no one will
+                 * ever do this, because no interrupt related to this try_run
+                 * will ever come from hardware. */
+                s5p_mfc_clock_off();
+        }
+}
+
+
+void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
+{
+        struct s5p_mfc_buf *b;
+        int i;
+
+        while (!list_empty(lh)) {
+                b = list_entry(lh->next, struct s5p_mfc_buf, list);
+                for (i = 0; i < b->b->num_planes; i++)
+                        vb2_set_plane_payload(b->b, i, 0);
+                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+                list_del(&b->list);
+        }
+}
+
+void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+        mfc_write(dev, 0xffff, S5P_FIMV_SI_RTN_CHID);
+}
+
+int s5p_mfc_get_dspl_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DISPLAY_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+int s5p_mfc_get_dec_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DECODE_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+int s5p_mfc_get_dspl_status_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DISPLAY_STATUS);
+}
+
+int s5p_mfc_get_dec_status_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DECODE_STATUS);
+}
+
+int s5p_mfc_get_dec_frame_type_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_DECODE_FRAME_TYPE) &
+                S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+int s5p_mfc_get_disp_frame_type_v5(struct s5p_mfc_ctx *ctx)
+{
+        return (s5p_mfc_read_info_v5(ctx, DISP_PIC_FRAME_TYPE) >>
+                        S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT) &
+                        S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+int s5p_mfc_get_consumed_stream_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_CONSUMED_BYTES);
+}
+
+int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
+{
+        int reason;
+        reason = mfc_read(dev, S5P_FIMV_RISC2HOST_CMD) &
+                S5P_FIMV_RISC2HOST_CMD_MASK;
+        switch (reason) {
+        case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
+                reason = S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+                reason = S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+                reason = S5P_MFC_R2H_CMD_SEQ_DONE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+                reason = S5P_MFC_R2H_CMD_FRAME_DONE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
+                reason = S5P_MFC_R2H_CMD_SLICE_DONE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
+                reason = S5P_MFC_R2H_CMD_SYS_INIT_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
+                reason = S5P_MFC_R2H_CMD_FW_STATUS_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SLEEP_RET:
+                reason = S5P_MFC_R2H_CMD_SLEEP_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+                reason = S5P_MFC_R2H_CMD_WAKEUP_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+                reason = S5P_MFC_R2H_CMD_INIT_BUFFERS_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+                reason = S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_ERR_RET:
+                reason = S5P_MFC_R2H_CMD_ERR_RET;
+                break;
+        default:
+                reason = S5P_MFC_R2H_CMD_EMPTY;
+        };
+        return reason;
+}
+
+int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG2);
+}
+
+int s5p_mfc_err_dec_v5(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DEC_MASK) >> S5P_FIMV_ERR_DEC_SHIFT;
+}
+
+int s5p_mfc_err_dspl_v5(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DSPL_MASK) >> S5P_FIMV_ERR_DSPL_SHIFT;
+}
+
+int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_HRESOL);
+}
+
+int s5p_mfc_get_img_height_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_VRESOL);
+}
+
+int s5p_mfc_get_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_BUF_NUMBER);
+}
+
+int s5p_mfc_get_mv_count_v5(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+        return -1;
+}
+
+int s5p_mfc_get_inst_no_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG1);
+}
+
+int s5p_mfc_get_enc_strm_size_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ENC_SI_STRM_SIZE);
+}
+
+int s5p_mfc_get_enc_slice_type_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ENC_SI_SLICE_TYPE);
+}
+
+int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+        return -1;
+}
+
+int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT);
+}
+
+int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, FRAME_PACK_SEI_AVAIL);
+}
+
+int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
+{
+        return -1;
+}
+
+int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
+{
+        return -1;
+}
+
+unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, PIC_TIME_TOP);
+}
+
+unsigned int s5p_mfc_get_pic_type_bot_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, PIC_TIME_BOT);
+}
+
+unsigned int s5p_mfc_get_crop_info_h_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, CROP_INFO_H);
+}
+
+unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, CROP_INFO_V);
+}
+
+/* Initialize opr function pointers for MFC v5 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
+        .alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v5,
+        .release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v5,
+        .alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v5,
+        .release_codec_buffers = s5p_mfc_release_codec_buffers_v5,
+        .alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v5,
+        .release_instance_buffer = s5p_mfc_release_instance_buffer_v5,
+        .alloc_dev_context_buffer = s5p_mfc_alloc_dev_context_buffer_v5,
+        .release_dev_context_buffer = s5p_mfc_release_dev_context_buffer_v5,
+        .dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v5,
+        .enc_calc_src_size = s5p_mfc_enc_calc_src_size_v5,
+        .set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v5,
+        .set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v5,
+        .set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v5,
+        .set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v5,
+        .get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v5,
+        .set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v5,
+        .init_decode = s5p_mfc_init_decode_v5,
+        .init_encode = s5p_mfc_init_encode_v5,
+        .encode_one_frame = s5p_mfc_encode_one_frame_v5,
+        .try_run = s5p_mfc_try_run_v5,
+        .cleanup_queue = s5p_mfc_cleanup_queue_v5,
+        .clear_int_flags = s5p_mfc_clear_int_flags_v5,
+        .write_info = s5p_mfc_write_info_v5,
+        .read_info = s5p_mfc_read_info_v5,
+        .get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v5,
+        .get_dec_y_adr = s5p_mfc_get_dec_y_adr_v5,
+        .get_dspl_status = s5p_mfc_get_dspl_status_v5,
+        .get_dec_status = s5p_mfc_get_dec_status_v5,
+        .get_dec_frame_type = s5p_mfc_get_dec_frame_type_v5,
+        .get_disp_frame_type = s5p_mfc_get_disp_frame_type_v5,
+        .get_consumed_stream = s5p_mfc_get_consumed_stream_v5,
+        .get_int_reason = s5p_mfc_get_int_reason_v5,
+        .get_int_err = s5p_mfc_get_int_err_v5,
+        .err_dec = s5p_mfc_err_dec_v5,
+        .err_dspl = s5p_mfc_err_dspl_v5,
+        .get_img_width = s5p_mfc_get_img_width_v5,
+        .get_img_height = s5p_mfc_get_img_height_v5,
+        .get_dpb_count = s5p_mfc_get_dpb_count_v5,
+        .get_mv_count = s5p_mfc_get_mv_count_v5,
+        .get_inst_no = s5p_mfc_get_inst_no_v5,
+        .get_enc_strm_size = s5p_mfc_get_enc_strm_size_v5,
+        .get_enc_slice_type = s5p_mfc_get_enc_slice_type_v5,
+        .get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v5,
+        .get_enc_pic_count = s5p_mfc_get_enc_pic_count_v5,
+        .get_sei_avail_status = s5p_mfc_get_sei_avail_status_v5,
+        .get_mvc_num_views = s5p_mfc_get_mvc_num_views_v5,
+        .get_mvc_view_id = s5p_mfc_get_mvc_view_id_v5,
+        .get_pic_type_top = s5p_mfc_get_pic_type_top_v5,
+        .get_pic_type_bot = s5p_mfc_get_pic_type_bot_v5,
+        .get_crop_info_h = s5p_mfc_get_crop_info_h_v5,
+        .get_crop_info_v = s5p_mfc_get_crop_info_v_v5,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void)
+{
+        return &s5p_mfc_ops_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
index 416ebd7ba35a..ffee39a127d5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
@@ -1,17 +1,22 @@
 /*
- * linux/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.h
  *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com/
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
  */
 
-#ifndef S5P_MFC_SHM_H_
-#define S5P_MFC_SHM_H_
+#ifndef S5P_MFC_OPR_V5_H_
+#define S5P_MFC_OPR_V5_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
 
 enum MFC_SHM_OFS {
         EXTENEDED_DECODE_STATUS = 0x00, /* D */
@@ -71,20 +76,10 @@ enum MFC_SHM_OFS {
         DBG_HISTORY_INPUT1      = 0xD4, /* C */
         DBG_HISTORY_OUTPUT      = 0xD8, /* C */
         HIERARCHICAL_P_QP       = 0xE0, /* E, H.264 */
+        FRAME_PACK_SEI_ENABLE   = 0x168, /* C */
+        FRAME_PACK_SEI_AVAIL    = 0x16c, /* D */
+        FRAME_PACK_SEI_INFO     = 0x17c, /* E */
 };
 
-int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx);
-
-#define s5p_mfc_write_shm(ctx, x, ofs)          \
-        do {                                    \
-                writel(x, (ctx->shm + ofs));    \
-                wmb();                          \
-        } while (0)
-
-static inline u32 s5p_mfc_read_shm(struct s5p_mfc_ctx *ctx, unsigned int ofs)
-{
-        rmb();
-        return readl(ctx->shm + ofs);
-}
-
-#endif /* S5P_MFC_SHM_H_ */
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void);
+#endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
new file mode 100644
index 000000000000..50b5bee3c44e
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -0,0 +1,1956 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#undef DEBUG
+
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/firmware.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/cacheflush.h>
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v6.h"
+
+/* #define S5P_MFC_DEBUG_REGWRITE  */
+#ifdef S5P_MFC_DEBUG_REGWRITE
+#undef writel
+#define writel(v, r)                                                    \
+        do {                                                            \
+                pr_err("MFCWRITE(%p): %08x\n", r, (unsigned int)v);     \
+        __raw_writel(v, r);                                             \
+        } while (0)
+#endif /* S5P_MFC_DEBUG_REGWRITE */
+
+#define READL(offset)           readl(dev->regs_base + (offset))
+#define WRITEL(data, offset)    writel((data), dev->regs_base + (offset))
+#define OFFSETA(x)              (((x) - dev->port_a) >> S5P_FIMV_MEM_OFFSET)
+#define OFFSETB(x)              (((x) - dev->port_b) >> S5P_FIMV_MEM_OFFSET)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+        /* NOP */
+
+        return 0;
+}
+
+/* Release temproary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        /* NOP */
+}
+
+int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+        return -1;
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int mb_width, mb_height;
+
+        mb_width = MB_WIDTH(ctx->img_width);
+        mb_height = MB_HEIGHT(ctx->img_height);
+
+        if (ctx->type == MFCINST_DECODER) {
+                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+        } else if (ctx->type == MFCINST_ENCODER) {
+                ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+                        ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V6(mb_width, mb_height),
+                        S5P_FIMV_TMV_BUFFER_ALIGN_V6);
+                ctx->luma_dpb_size = ALIGN((mb_width * mb_height) *
+                                S5P_FIMV_LUMA_MB_TO_PIXEL_V6,
+                                S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6);
+                ctx->chroma_dpb_size = ALIGN((mb_width * mb_height) *
+                                S5P_FIMV_CHROMA_MB_TO_PIXEL_V6,
+                                S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6);
+                ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V6(
+                                        ctx->img_width, ctx->img_height,
+                                        mb_width, mb_height),
+                                        S5P_FIMV_ME_BUFFER_ALIGN_V6);
+
+                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+                          ctx->luma_dpb_size, ctx->chroma_dpb_size);
+        } else {
+                return -EINVAL;
+        }
+
+        /* Codecs have different memory requirements */
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+        case S5P_MFC_CODEC_H264_MVC_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size =
+                        ctx->scratch_buf_size +
+                        (ctx->mv_count * ctx->mv_size);
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+        case S5P_MFC_CODEC_VC1_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                ctx->bank1_size = 0;
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_VP8_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size =
+                        ctx->scratch_buf_size + ctx->tmv_buffer_size +
+                        (ctx->dpb_count * (ctx->luma_dpb_size +
+                        ctx->chroma_dpb_size + ctx->me_buffer_size));
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+        case S5P_MFC_CODEC_H263_ENC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size =
+                        ctx->scratch_buf_size + ctx->tmv_buffer_size +
+                        (ctx->dpb_count * (ctx->luma_dpb_size +
+                        ctx->chroma_dpb_size + ctx->me_buffer_size));
+                ctx->bank2_size = 0;
+                break;
+        default:
+                break;
+        }
+
+        /* Allocate only if memory from bank 1 is necessary */
+        if (ctx->bank1_size > 0) {
+                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+                if (IS_ERR(ctx->bank1_buf)) {
+                        ctx->bank1_buf = 0;
+                        pr_err("Buf alloc for decoding failed (port A)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank1_phys = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        }
+
+        return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->bank1_buf) {
+                vb2_dma_contig_memops.put(ctx->bank1_buf);
+                ctx->bank1_buf = 0;
+                ctx->bank1_phys = 0;
+                ctx->bank1_size = 0;
+        }
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+        mfc_debug_enter();
+
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+        case S5P_MFC_CODEC_H264_MVC_DEC:
+                ctx->ctx.size = buf_size->h264_dec_ctx;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+        case S5P_MFC_CODEC_H263_DEC:
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+        case S5P_MFC_CODEC_VC1_DEC:
+        case S5P_MFC_CODEC_MPEG2_DEC:
+        case S5P_MFC_CODEC_VP8_DEC:
+                ctx->ctx.size = buf_size->other_dec_ctx;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                ctx->ctx.size = buf_size->h264_enc_ctx;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+        case S5P_MFC_CODEC_H263_ENC:
+                ctx->ctx.size = buf_size->other_enc_ctx;
+                break;
+        default:
+                ctx->ctx.size = 0;
+                mfc_err("Codec type(%d) should be checked!\n", ctx->codec_mode);
+                break;
+        }
+
+        ctx->ctx.alloc = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.size);
+        if (IS_ERR(ctx->ctx.alloc)) {
+                mfc_err("Allocating context buffer failed.\n");
+                return PTR_ERR(ctx->ctx.alloc);
+        }
+
+        ctx->ctx.dma = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.alloc);
+
+        ctx->ctx.virt = vb2_dma_contig_memops.vaddr(ctx->ctx.alloc);
+        if (!ctx->ctx.virt) {
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.dma = 0;
+                ctx->ctx.virt = NULL;
+
+                mfc_err("Remapping context buffer failed.\n");
+                return -ENOMEM;
+        }
+
+        memset(ctx->ctx.virt, 0, ctx->ctx.size);
+        wmb();
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        mfc_debug_enter();
+
+        if (ctx->ctx.alloc) {
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.dma = 0;
+                ctx->ctx.virt = NULL;
+        }
+
+        mfc_debug_leave();
+}
+
+/* Allocate context buffers for SYS_INIT */
+int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+        mfc_debug_enter();
+
+        dev->ctx_buf.alloc = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], buf_size->dev_ctx);
+        if (IS_ERR(dev->ctx_buf.alloc)) {
+                mfc_err("Allocating DESC buffer failed.\n");
+                return PTR_ERR(dev->ctx_buf.alloc);
+        }
+
+        dev->ctx_buf.dma = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX],
+                        dev->ctx_buf.alloc);
+
+        dev->ctx_buf.virt = vb2_dma_contig_memops.vaddr(dev->ctx_buf.alloc);
+        if (!dev->ctx_buf.virt) {
+                vb2_dma_contig_memops.put(dev->ctx_buf.alloc);
+                dev->ctx_buf.alloc = NULL;
+                dev->ctx_buf.dma = 0;
+
+                mfc_err("Remapping DESC buffer failed.\n");
+                return -ENOMEM;
+        }
+
+        memset(dev->ctx_buf.virt, 0, buf_size->dev_ctx);
+        wmb();
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+/* Release context buffers for SYS_INIT */
+void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+        if (dev->ctx_buf.alloc) {
+                vb2_dma_contig_memops.put(dev->ctx_buf.alloc);
+                dev->ctx_buf.alloc = NULL;
+                dev->ctx_buf.dma = 0;
+                dev->ctx_buf.virt = NULL;
+        }
+}
+
+static int calc_plane(int width, int height)
+{
+        int mbX, mbY;
+
+        mbX = DIV_ROUND_UP(width, S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+        mbY = DIV_ROUND_UP(height, S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6);
+
+        if (width * height < S5P_FIMV_MAX_FRAME_SIZE_V6)
+                mbY = (mbY + 1) / 2 * 2;
+
+        return (mbX * S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6) *
+                (mbY * S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+}
+
+void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
+{
+        ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN_V6);
+        ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN_V6);
+        mfc_debug(2, "SEQ Done: Movie dimensions %dx%d,\n"
+                        "buffer dimensions: %dx%d\n", ctx->img_width,
+                        ctx->img_height, ctx->buf_width, ctx->buf_height);
+
+        ctx->luma_size = calc_plane(ctx->img_width, ctx->img_height);
+        ctx->chroma_size = calc_plane(ctx->img_width, (ctx->img_height >> 1));
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+                ctx->mv_size = S5P_MFC_DEC_MV_SIZE_V6(ctx->img_width,
+                                ctx->img_height);
+                ctx->mv_size = ALIGN(ctx->mv_size, 16);
+        } else {
+                ctx->mv_size = 0;
+        }
+}
+
+void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int mb_width, mb_height;
+
+        mb_width = MB_WIDTH(ctx->img_width);
+        mb_height = MB_HEIGHT(ctx->img_height);
+
+        ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN_V6);
+        ctx->luma_size = ALIGN((mb_width * mb_height) * 256, 256);
+        ctx->chroma_size = ALIGN((mb_width * mb_height) * 128, 256);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx, int buf_addr,
+                  unsigned int start_num_byte, unsigned int strm_size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
+
+        mfc_debug_enter();
+        mfc_debug(2, "inst_no: %d, buf_addr: 0x%08x,\n"
+                "buf_size: 0x%08x (%d)\n",
+                ctx->inst_no, buf_addr, strm_size, strm_size);
+        WRITEL(strm_size, S5P_FIMV_D_STREAM_DATA_SIZE_V6);
+        WRITEL(buf_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V6);
+        WRITEL(buf_size->cpb, S5P_FIMV_D_CPB_BUFFER_SIZE_V6);
+        WRITEL(start_num_byte, S5P_FIMV_D_CPB_BUFFER_OFFSET_V6);
+
+        mfc_debug_leave();
+        return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int frame_size, i;
+        unsigned int frame_size_ch, frame_size_mv;
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1;
+        int buf_size1;
+        int align_gap;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+
+        mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+        mfc_debug(2, "Total DPB COUNT: %d\n", ctx->total_dpb_count);
+        mfc_debug(2, "Setting display delay to %d\n", ctx->display_delay);
+
+        WRITEL(ctx->total_dpb_count, S5P_FIMV_D_NUM_DPB_V6);
+        WRITEL(ctx->luma_size, S5P_FIMV_D_LUMA_DPB_SIZE_V6);
+        WRITEL(ctx->chroma_size, S5P_FIMV_D_CHROMA_DPB_SIZE_V6);
+
+        WRITEL(buf_addr1, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6);
+        WRITEL(ctx->scratch_buf_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6);
+        buf_addr1 += ctx->scratch_buf_size;
+        buf_size1 -= ctx->scratch_buf_size;
+
+        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_FIMV_CODEC_H264_MVC_DEC){
+                WRITEL(ctx->mv_size, S5P_FIMV_D_MV_BUFFER_SIZE_V6);
+                WRITEL(ctx->mv_count, S5P_FIMV_D_NUM_MV_V6);
+        }
+
+        frame_size = ctx->luma_size;
+        frame_size_ch = ctx->chroma_size;
+        frame_size_mv = ctx->mv_size;
+        mfc_debug(2, "Frame size: %d ch: %d mv: %d\n",
+                        frame_size, frame_size_ch, frame_size_mv);
+
+        for (i = 0; i < ctx->total_dpb_count; i++) {
+                /* Bank2 */
+                mfc_debug(2, "Luma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.luma);
+                WRITEL(ctx->dst_bufs[i].cookie.raw.luma,
+                                S5P_FIMV_D_LUMA_DPB_V6 + i * 4);
+                mfc_debug(2, "\tChroma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.chroma);
+                WRITEL(ctx->dst_bufs[i].cookie.raw.chroma,
+                                S5P_FIMV_D_CHROMA_DPB_V6 + i * 4);
+        }
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+                for (i = 0; i < ctx->mv_count; i++) {
+                        /* To test alignment */
+                        align_gap = buf_addr1;
+                        buf_addr1 = ALIGN(buf_addr1, 16);
+                        align_gap = buf_addr1 - align_gap;
+                        buf_size1 -= align_gap;
+
+                        mfc_debug(2, "\tBuf1: %x, size: %d\n",
+                                        buf_addr1, buf_size1);
+                        WRITEL(buf_addr1, S5P_FIMV_D_MV_BUFFER_V6 + i * 4);
+                        buf_addr1 += frame_size_mv;
+                        buf_size1 -= frame_size_mv;
+                }
+        }
+
+        mfc_debug(2, "Buf1: %u, buf_size1: %d (frames %d)\n",
+                        buf_addr1, buf_size1, ctx->total_dpb_count);
+        if (buf_size1 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated.\n");
+                return -ENOMEM;
+        }
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+        mfc_debug(2, "After setting buffers.\n");
+        return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+                unsigned long addr, unsigned int size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        WRITEL(addr, S5P_FIMV_E_STREAM_BUFFER_ADDR_V6); /* 16B align */
+        WRITEL(size, S5P_FIMV_E_STREAM_BUFFER_SIZE_V6);
+
+        mfc_debug(2, "stream buf addr: 0x%08lx, size: 0x%d",
+                addr, size);
+
+        return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+                unsigned long y_addr, unsigned long c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        WRITEL(y_addr, S5P_FIMV_E_SOURCE_LUMA_ADDR_V6); /* 256B align */
+        WRITEL(c_addr, S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6);
+
+        mfc_debug(2, "enc src y buf addr: 0x%08lx", y_addr);
+        mfc_debug(2, "enc src c buf addr: 0x%08lx", c_addr);
+}
+
+void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+                unsigned long *y_addr, unsigned long *c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long enc_recon_y_addr, enc_recon_c_addr;
+
+        *y_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6);
+        *c_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6);
+
+        enc_recon_y_addr = READL(S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6);
+        enc_recon_c_addr = READL(S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6);
+
+        mfc_debug(2, "recon y addr: 0x%08lx", enc_recon_y_addr);
+        mfc_debug(2, "recon c addr: 0x%08lx", enc_recon_c_addr);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1, buf_size1;
+        int i;
+
+        mfc_debug_enter();
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+
+        mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+
+        for (i = 0; i < ctx->dpb_count; i++) {
+                WRITEL(buf_addr1, S5P_FIMV_E_LUMA_DPB_V6 + (4 * i));
+                buf_addr1 += ctx->luma_dpb_size;
+                WRITEL(buf_addr1, S5P_FIMV_E_CHROMA_DPB_V6 + (4 * i));
+                buf_addr1 += ctx->chroma_dpb_size;
+                WRITEL(buf_addr1, S5P_FIMV_E_ME_BUFFER_V6 + (4 * i));
+                buf_addr1 += ctx->me_buffer_size;
+                buf_size1 -= (ctx->luma_dpb_size + ctx->chroma_dpb_size +
+                        ctx->me_buffer_size);
+        }
+
+        WRITEL(buf_addr1, S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6);
+        WRITEL(ctx->scratch_buf_size, S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6);
+        buf_addr1 += ctx->scratch_buf_size;
+        buf_size1 -= ctx->scratch_buf_size;
+
+        WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER0_V6);
+        buf_addr1 += ctx->tmv_buffer_size >> 1;
+        WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER1_V6);
+        buf_addr1 += ctx->tmv_buffer_size >> 1;
+        buf_size1 -= ctx->tmv_buffer_size;
+
+        mfc_debug(2, "Buf1: %u, buf_size1: %d (ref frames %d)\n",
+                        buf_addr1, buf_size1, ctx->dpb_count);
+        if (buf_size1 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated.\n");
+                return -ENOMEM;
+        }
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        WRITEL(ctx->slice_mode, S5P_FIMV_E_MSLICE_MODE_V6);
+        if (ctx->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                WRITEL(ctx->slice_size.mb, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+        } else if (ctx->slice_mode ==
+                        V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                WRITEL(ctx->slice_size.bits, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+        } else {
+                WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+                WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+        }
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        unsigned int reg = 0;
+
+        mfc_debug_enter();
+
+        /* width */
+        WRITEL(ctx->img_width, S5P_FIMV_E_FRAME_WIDTH_V6); /* 16 align */
+        /* height */
+        WRITEL(ctx->img_height, S5P_FIMV_E_FRAME_HEIGHT_V6); /* 16 align */
+
+        /* cropped width */
+        WRITEL(ctx->img_width, S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6);
+        /* cropped height */
+        WRITEL(ctx->img_height, S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+        /* cropped offset */
+        WRITEL(0x0, S5P_FIMV_E_FRAME_CROP_OFFSET_V6);
+
+        /* pictype : IDR period */
+        reg = 0;
+        reg |= p->gop_size & 0xFFFF;
+        WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        ctx->slice_mode = p->slice_mode;
+        reg = 0;
+        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                reg |= (0x1 << 3);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                ctx->slice_size.mb = p->slice_mb;
+        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                reg |= (0x1 << 3);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                ctx->slice_size.bits = p->slice_bit;
+        } else {
+                reg &= ~(0x1 << 3);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+        }
+
+        s5p_mfc_set_slice_mode(ctx);
+
+        /* cyclic intra refresh */
+        WRITEL(p->intra_refresh_mb, S5P_FIMV_E_IR_SIZE_V6);
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        if (p->intra_refresh_mb == 0)
+                reg &= ~(0x1 << 4);
+        else
+                reg |= (0x1 << 4);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* 'NON_REFERENCE_STORE_ENABLE' for debugging */
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        reg &= ~(0x1 << 9);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+                /* 0: Linear, 1: 2D tiled*/
+                reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+                reg &= ~(0x1 << 7);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                /* 0: NV12(CbCr), 1: NV21(CrCb) */
+                WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+        } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV21M) {
+                /* 0: Linear, 1: 2D tiled*/
+                reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+                reg &= ~(0x1 << 7);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                /* 0: NV12(CbCr), 1: NV21(CrCb) */
+                WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+        } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
+                /* 0: Linear, 1: 2D tiled*/
+                reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+                reg |= (0x1 << 7);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                /* 0: NV12(CbCr), 1: NV21(CrCb) */
+                WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+        }
+
+        /* memory structure recon. frame */
+        /* 0: Linear, 1: 2D tiled */
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        reg |= (0x1 << 8);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* padding control & value */
+        WRITEL(0x0, S5P_FIMV_E_PADDING_CTRL_V6);
+        if (p->pad) {
+                reg = 0;
+                /** enable */
+                reg |= (1 << 31);
+                /** cr value */
+                reg |= ((p->pad_cr & 0xFF) << 16);
+                /** cb value */
+                reg |= ((p->pad_cb & 0xFF) << 8);
+                /** y value */
+                reg |= p->pad_luma & 0xFF;
+                WRITEL(reg, S5P_FIMV_E_PADDING_CTRL_V6);
+        }
+
+        /* rate control config. */
+        reg = 0;
+        /* frame-level rate control */
+        reg |= ((p->rc_frame & 0x1) << 9);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* bit rate */
+        if (p->rc_frame)
+                WRITEL(p->rc_bitrate,
+                        S5P_FIMV_E_RC_BIT_RATE_V6);
+        else
+                WRITEL(1, S5P_FIMV_E_RC_BIT_RATE_V6);
+
+        /* reaction coefficient */
+        if (p->rc_frame) {
+                if (p->rc_reaction_coeff < TIGHT_CBR_MAX) /* tight CBR */
+                        WRITEL(1, S5P_FIMV_E_RC_RPARAM_V6);
+                else                                      /* loose CBR */
+                        WRITEL(2, S5P_FIMV_E_RC_RPARAM_V6);
+        }
+
+        /* seq header ctrl */
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        reg &= ~(0x1 << 2);
+        reg |= ((p->seq_hdr_mode & 0x1) << 2);
+
+        /* frame skip mode */
+        reg &= ~(0x3);
+        reg |= (p->frame_skip_mode & 0x3);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* 'DROP_CONTROL_ENABLE', disable */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        reg &= ~(0x1 << 10);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* setting for MV range [16, 256] */
+        reg = 0;
+        reg &= ~(0x3FFF);
+        reg = 256;
+        WRITEL(reg, S5P_FIMV_E_MV_HOR_RANGE_V6);
+
+        reg = 0;
+        reg &= ~(0x3FFF);
+        reg = 256;
+        WRITEL(reg, S5P_FIMV_E_MV_VER_RANGE_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_FRAME_INSERTION_V6);
+        WRITEL(0x0, S5P_FIMV_E_ROI_BUFFER_ADDR_V6);
+        WRITEL(0x0, S5P_FIMV_E_PARAM_CHANGE_V6);
+        WRITEL(0x0, S5P_FIMV_E_RC_ROI_CTRL_V6);
+        WRITEL(0x0, S5P_FIMV_E_PICTURE_TAG_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_BIT_COUNT_ENABLE_V6);
+        WRITEL(0x0, S5P_FIMV_E_MAX_BIT_COUNT_V6);
+        WRITEL(0x0, S5P_FIMV_E_MIN_BIT_COUNT_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_ADDR_V6);
+        WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_SIZE_V6);
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+        unsigned int reg = 0;
+        int i;
+
+        mfc_debug_enter();
+
+        s5p_mfc_set_enc_params(ctx);
+
+        /* pictype : number of B */
+        reg = READL(S5P_FIMV_E_GOP_CONFIG_V6);
+        reg &= ~(0x3 << 16);
+        reg |= ((p->num_b_frame & 0x3) << 16);
+        WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+        /* profile & level */
+        reg = 0;
+        /** level */
+        reg |= ((p_h264->level & 0xFF) << 8);
+        /** profile - 0 ~ 3 */
+        reg |= p_h264->profile & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+        /* rate control config. */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        /** macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= ((p->rc_mb & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+        /** frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h264->rc_frame_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* max & min value of QP */
+        reg = 0;
+        /** max QP */
+        reg |= ((p_h264->rc_max_qp & 0x3F) << 8);
+        /** min QP */
+        reg |= p_h264->rc_min_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+        /* other QPs */
+        WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        if (!p->rc_frame && !p->rc_mb) {
+                reg = 0;
+                reg |= ((p_h264->rc_b_frame_qp & 0x3F) << 16);
+                reg |= ((p_h264->rc_p_frame_qp & 0x3F) << 8);
+                reg |= p_h264->rc_frame_qp & 0x3F;
+                WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        }
+
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+                reg = 0;
+                reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+                reg |= p->rc_framerate_denom & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+        }
+
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                WRITEL(p_h264->cpb_size & 0xFFFF,
+                                S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+                if (p->rc_frame)
+                        WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+        }
+
+        /* interlace */
+        reg = 0;
+        reg |= ((p_h264->interlace & 0x1) << 3);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* height */
+        if (p_h264->interlace) {
+                WRITEL(ctx->img_height >> 1,
+                                S5P_FIMV_E_FRAME_HEIGHT_V6); /* 32 align */
+                /* cropped height */
+                WRITEL(ctx->img_height >> 1,
+                                S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+        }
+
+        /* loop filter ctrl */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x3 << 1);
+        reg |= ((p_h264->loop_filter_mode & 0x3) << 1);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* loopfilter alpha offset */
+        if (p_h264->loop_filter_alpha < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_h264->loop_filter_alpha) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_h264->loop_filter_alpha & 0xF);
+        }
+        WRITEL(reg, S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6);
+
+        /* loopfilter beta offset */
+        if (p_h264->loop_filter_beta < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_h264->loop_filter_beta) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_h264->loop_filter_beta & 0xF);
+        }
+        WRITEL(reg, S5P_FIMV_E_H264_LF_BETA_OFFSET_V6);
+
+        /* entropy coding mode */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1);
+        reg |= p_h264->entropy_mode & 0x1;
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* number of ref. picture */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 7);
+        reg |= (((p_h264->num_ref_pic_4p - 1) & 0x1) << 7);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* 8x8 transform enable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x3 << 12);
+        reg |= ((p_h264->_8x8_transform & 0x3) << 12);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* macroblock adaptive scaling features */
+        WRITEL(0x0, S5P_FIMV_E_MB_RC_CONFIG_V6);
+        if (p->rc_mb) {
+                reg = 0;
+                /** dark region */
+                reg |= ((p_h264->rc_mb_dark & 0x1) << 3);
+                /** smooth region */
+                reg |= ((p_h264->rc_mb_smooth & 0x1) << 2);
+                /** static region */
+                reg |= ((p_h264->rc_mb_static & 0x1) << 1);
+                /** high activity region */
+                reg |= p_h264->rc_mb_activity & 0x1;
+                WRITEL(reg, S5P_FIMV_E_MB_RC_CONFIG_V6);
+        }
+
+        /* aspect ratio VUI */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 5);
+        reg |= ((p_h264->vui_sar & 0x1) << 5);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_ASPECT_RATIO_V6);
+        WRITEL(0x0, S5P_FIMV_E_EXTENDED_SAR_V6);
+        if (p_h264->vui_sar) {
+                /* aspect ration IDC */
+                reg = 0;
+                reg |= p_h264->vui_sar_idc & 0xFF;
+                WRITEL(reg, S5P_FIMV_E_ASPECT_RATIO_V6);
+                if (p_h264->vui_sar_idc == 0xFF) {
+                        /* extended SAR */
+                        reg = 0;
+                        reg |= (p_h264->vui_ext_sar_width & 0xFFFF) << 16;
+                        reg |= p_h264->vui_ext_sar_height & 0xFFFF;
+                        WRITEL(reg, S5P_FIMV_E_EXTENDED_SAR_V6);
+                }
+        }
+
+        /* intra picture period for H.264 open GOP */
+        /* control */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 4);
+        reg |= ((p_h264->open_gop & 0x1) << 4);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+        /* value */
+        WRITEL(0x0, S5P_FIMV_E_H264_I_PERIOD_V6);
+        if (p_h264->open_gop) {
+                reg = 0;
+                reg |= p_h264->open_gop_size & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_H264_I_PERIOD_V6);
+        }
+
+        /* 'WEIGHTED_BI_PREDICTION' for B is disable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x3 << 9);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* 'CONSTRAINED_INTRA_PRED_ENABLE' is disable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 14);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* ASO */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 6);
+        reg |= ((p_h264->aso & 0x1) << 6);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* hier qp enable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 8);
+        reg |= ((p_h264->open_gop & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+        reg = 0;
+        if (p_h264->hier_qp && p_h264->hier_qp_layer) {
+                reg |= (p_h264->hier_qp_type & 0x1) << 0x3;
+                reg |= p_h264->hier_qp_layer & 0x7;
+                WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+                /* QP value for each layer */
+                for (i = 0; i < (p_h264->hier_qp_layer & 0x7); i++)
+                        WRITEL(p_h264->hier_qp_layer_qp[i],
+                                S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6 +
+                                i * 4);
+        }
+        /* number of coding layer should be zero when hierarchical is disable */
+        WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+
+        /* frame packing SEI generation */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 25);
+        reg |= ((p_h264->sei_frame_packing & 0x1) << 25);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+        if (p_h264->sei_frame_packing) {
+                reg = 0;
+                /** current frame0 flag */
+                reg |= ((p_h264->sei_fp_curr_frame_0 & 0x1) << 2);
+                /** arrangement type */
+                reg |= p_h264->sei_fp_arrangement_type & 0x3;
+                WRITEL(reg, S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6);
+        }
+
+        if (p_h264->fmo) {
+                switch (p_h264->fmo_map_type) {
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_INTERLEAVED_SLICES:
+                        if (p_h264->fmo_slice_grp > 4)
+                                p_h264->fmo_slice_grp = 4;
+                        for (i = 0; i < (p_h264->fmo_slice_grp & 0xF); i++)
+                                WRITEL(p_h264->fmo_run_len[i] - 1,
+                                S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6 +
+                                i * 4);
+                        break;
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES:
+                        if (p_h264->fmo_slice_grp > 4)
+                                p_h264->fmo_slice_grp = 4;
+                        break;
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_RASTER_SCAN:
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN:
+                        if (p_h264->fmo_slice_grp > 2)
+                                p_h264->fmo_slice_grp = 2;
+                        WRITEL(p_h264->fmo_chg_dir & 0x1,
+                                S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6);
+                        /* the valid range is 0 ~ number of macroblocks -1 */
+                        WRITEL(p_h264->fmo_chg_rate,
+                                S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6);
+                        break;
+                default:
+                        mfc_err("Unsupported map type for FMO: %d\n",
+                                        p_h264->fmo_map_type);
+                        p_h264->fmo_map_type = 0;
+                        p_h264->fmo_slice_grp = 1;
+                        break;
+                }
+
+                WRITEL(p_h264->fmo_map_type,
+                                S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6);
+                WRITEL(p_h264->fmo_slice_grp - 1,
+                                S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+        } else {
+                WRITEL(0, S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+        }
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+        unsigned int reg = 0;
+
+        mfc_debug_enter();
+
+        s5p_mfc_set_enc_params(ctx);
+
+        /* pictype : number of B */
+        reg = READL(S5P_FIMV_E_GOP_CONFIG_V6);
+        reg &= ~(0x3 << 16);
+        reg |= ((p->num_b_frame & 0x3) << 16);
+        WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+        /* profile & level */
+        reg = 0;
+        /** level */
+        reg |= ((p_mpeg4->level & 0xFF) << 8);
+        /** profile - 0 ~ 1 */
+        reg |= p_mpeg4->profile & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+        /* rate control config. */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        /** macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= ((p->rc_mb & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+        /** frame QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_frame_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* max & min value of QP */
+        reg = 0;
+        /** max QP */
+        reg |= ((p_mpeg4->rc_max_qp & 0x3F) << 8);
+        /** min QP */
+        reg |= p_mpeg4->rc_min_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+        /* other QPs */
+        WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        if (!p->rc_frame && !p->rc_mb) {
+                reg = 0;
+                reg |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 16);
+                reg |= ((p_mpeg4->rc_p_frame_qp & 0x3F) << 8);
+                reg |= p_mpeg4->rc_frame_qp & 0x3F;
+                WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        }
+
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+                reg = 0;
+                reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+                reg |= p->rc_framerate_denom & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+        }
+
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                WRITEL(p->vbv_size & 0xFFFF, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+                if (p->rc_frame)
+                        WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+        }
+
+        /* Disable HEC */
+        WRITEL(0x0, S5P_FIMV_E_MPEG4_OPTIONS_V6);
+        WRITEL(0x0, S5P_FIMV_E_MPEG4_HEC_PERIOD_V6);
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+        unsigned int reg = 0;
+
+        mfc_debug_enter();
+
+        s5p_mfc_set_enc_params(ctx);
+
+        /* profile & level */
+        reg = 0;
+        /** profile */
+        reg |= (0x1 << 4);
+        WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+        /* rate control config. */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        /** macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= ((p->rc_mb & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+        /** frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_frame_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* max & min value of QP */
+        reg = 0;
+        /** max QP */
+        reg |= ((p_h263->rc_max_qp & 0x3F) << 8);
+        /** min QP */
+        reg |= p_h263->rc_min_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+        /* other QPs */
+        WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        if (!p->rc_frame && !p->rc_mb) {
+                reg = 0;
+                reg |= ((p_h263->rc_b_frame_qp & 0x3F) << 16);
+                reg |= ((p_h263->rc_p_frame_qp & 0x3F) << 8);
+                reg |= p_h263->rc_frame_qp & 0x3F;
+                WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        }
+
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+                reg = 0;
+                reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+                reg |= p->rc_framerate_denom & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+        }
+
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                WRITEL(p->vbv_size & 0xFFFF, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+                if (p->rc_frame)
+                        WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+        }
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int reg = 0;
+        int fmo_aso_ctrl = 0;
+
+        mfc_debug_enter();
+        mfc_debug(2, "InstNo: %d/%d\n", ctx->inst_no,
+                        S5P_FIMV_CH_SEQ_HEADER_V6);
+        mfc_debug(2, "BUFs: %08x %08x %08x\n",
+                  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
+                  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
+                  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6));
+
+        /* FMO_ASO_CTRL - 0: Enable, 1: Disable */
+        reg |= (fmo_aso_ctrl << S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6);
+
+        /* When user sets desplay_delay to 0,
+         * It works as "display_delay enable" and delay set to 0.
+         * If user wants display_delay disable, It should be
+         * set to negative value. */
+        if (ctx->display_delay >= 0) {
+                reg |= (0x1 << S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6);
+                WRITEL(ctx->display_delay, S5P_FIMV_D_DISPLAY_DELAY_V6);
+        }
+        /* Setup loop filter, for decoding this is only valid for MPEG4 */
+        if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC) {
+                mfc_debug(2, "Set loop filter to: %d\n",
+                                ctx->loop_filter_mpeg4);
+                reg |= (ctx->loop_filter_mpeg4 <<
+                                S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6);
+        }
+        if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)
+                reg |= (0x1 << S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6);
+
+        WRITEL(reg, S5P_FIMV_D_DEC_OPTIONS_V6);
+
+        /* 0: NV12(CbCr), 1: NV21(CrCb) */
+        if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV21M)
+                WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+        else
+                WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+
+        /* sei parse */
+        WRITEL(ctx->sei_fp_parse & 0x1, S5P_FIMV_D_SEI_ENABLE_V6);
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+        mfc_debug_leave();
+        return 0;
+}
+
+static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+        if (flush)
+                dpb = READL(S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (1 << 14);
+        else
+                dpb = READL(S5P_FIMV_SI_CH0_DPB_CONF_CTRL) & ~(1 << 14);
+        WRITEL(dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
+                        enum s5p_mfc_decode_arg last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        WRITEL(ctx->dec_dst_flag, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6);
+        WRITEL(ctx->slice_interface & 0x1, S5P_FIMV_D_SLICE_IF_ENABLE_V6);
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        /* Issue different commands to instance basing on whether it
+         * is the last frame or not. */
+        switch (last_frame) {
+        case 0:
+                s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                                S5P_FIMV_CH_FRAME_START_V6, NULL);
+                break;
+        case 1:
+                s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                                S5P_FIMV_CH_LAST_FRAME_V6, NULL);
+                break;
+        default:
+                mfc_err("Unsupported last frame arg.\n");
+                return -EINVAL;
+        }
+
+        mfc_debug(2, "Decoding a usual frame.\n");
+        return 0;
+}
+
+int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                s5p_mfc_set_enc_params_h264(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+                s5p_mfc_set_enc_params_mpeg4(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+                s5p_mfc_set_enc_params_h263(ctx);
+        else {
+                mfc_err("Unknown codec for encoding (%x).\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+        return 0;
+}
+
+int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+        int i;
+
+        if (p_h264->aso) {
+                for (i = 0; i < 8; i++)
+                        WRITEL(p_h264->aso_slice_order[i],
+                                S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6 + i * 4);
+        }
+        return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_debug(2, "++\n");
+
+        /* memory structure cur. frame */
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                s5p_mfc_h264_set_aso_slice_order_v6(ctx);
+
+        s5p_mfc_set_slice_mode(ctx);
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_FRAME_START_V6, NULL);
+
+        mfc_debug(2, "--\n");
+
+        return 0;
+}
+
+static inline int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+        unsigned long flags;
+        int new_ctx;
+        int cnt;
+
+        spin_lock_irqsave(&dev->condlock, flags);
+        mfc_debug(2, "Previos context: %d (bits %08lx)\n", dev->curr_ctx,
+                                                        dev->ctx_work_bits);
+        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+        cnt = 0;
+        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+                cnt++;
+                if (cnt > MFC_NUM_CONTEXTS) {
+                        /* No contexts to run */
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                        return -EAGAIN;
+                }
+        }
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        return new_ctx;
+}
+
+static inline void s5p_mfc_run_dec_last_frames(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->flags |= MFC_BUF_FLAG_USED;
+        s5p_mfc_set_dec_stream_buffer_v6(ctx,
+                        vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), 0, 0);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_decode_one_frame_v6(ctx, 1);
+}
+
+static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+        int last_frame = 0;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->flags |= MFC_BUF_FLAG_USED;
+        s5p_mfc_set_dec_stream_buffer_v6(ctx,
+                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                        ctx->consumed_stream,
+                        temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        index = temp_vb->b->v4l2_buf.index;
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+                last_frame = 1;
+                mfc_debug(2, "Setting ctx->state to FINISHING\n");
+                ctx->state = MFCINST_FINISHING;
+        }
+        s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+
+        return 0;
+}
+
+static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        struct s5p_mfc_buf *src_mb;
+        unsigned long src_y_addr, src_c_addr, dst_addr;
+        /*
+        unsigned int src_y_size, src_c_size;
+        */
+        unsigned int dst_size;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "no src buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+
+        if (list_empty(&ctx->dst_queue)) {
+                mfc_debug(2, "no dst buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+
+        src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        src_mb->flags |= MFC_BUF_FLAG_USED;
+        src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
+        src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
+
+        mfc_debug(2, "enc src y addr: 0x%08lx", src_y_addr);
+        mfc_debug(2, "enc src c addr: 0x%08lx", src_c_addr);
+
+        s5p_mfc_set_enc_frame_buffer_v6(ctx, src_y_addr, src_c_addr);
+
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_mb->flags |= MFC_BUF_FLAG_USED;
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+
+        s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        index = src_mb->b->v4l2_buf.index;
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_encode_one_frame_v6(ctx);
+
+        return 0;
+}
+
+static inline void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+
+        /* Initializing decoding - parsing header */
+        spin_lock_irqsave(&dev->irqlock, flags);
+        mfc_debug(2, "Preparing to init decoding.\n");
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer_v6(ctx,
+                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), 0,
+                        temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_decode_v6(ctx);
+}
+
+static inline void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long dst_addr;
+        unsigned int dst_size;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_encode_v6(ctx);
+}
+
+static inline int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int ret;
+        /* Header was parsed now start processing
+         * First set the output frame buffers
+         * s5p_mfc_alloc_dec_buffers(ctx); */
+
+        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+                mfc_err("It seems that not all destionation buffers were\n"
+                        "mmaped.MFC requires that all destination are mmaped\n"
+                        "before starting processing.\n");
+                return -EAGAIN;
+        }
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_dec_frame_buffer_v6(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem.\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+static inline int s5p_mfc_run_init_enc_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int ret;
+
+        ret = s5p_mfc_alloc_codec_buffers_v6(ctx);
+        if (ret) {
+                mfc_err("Failed to allocate encoding buffers.\n");
+                return -ENOMEM;
+        }
+
+        /* Header was generated now starting processing
+         * First set the reference frame buffers
+         */
+        if (ctx->capture_state != QUEUE_BUFS_REQUESTED) {
+                mfc_err("It seems that destionation buffers were not\n"
+                        "requested.MFC requires that header should be generated\n"
+                        "before allocating codec buffer.\n");
+                return -EAGAIN;
+        }
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_enc_ref_buffer_v6(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem.\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_ctx *ctx;
+        int new_ctx;
+        unsigned int ret = 0;
+
+        mfc_debug(1, "Try run dev: %p\n", dev);
+
+        /* Check whether hardware is not running */
+        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+                /* This is perfectly ok, the scheduled ctx should wait */
+                mfc_debug(1, "Couldn't lock HW.\n");
+                return;
+        }
+
+        /* Choose the context to run */
+        new_ctx = s5p_mfc_get_new_ctx(dev);
+        if (new_ctx < 0) {
+                /* No contexts to run */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+                        mfc_err("Failed to unlock hardware.\n");
+                        return;
+                }
+
+                mfc_debug(1, "No ctx is scheduled to be run.\n");
+                return;
+        }
+
+        mfc_debug(1, "New context: %d\n", new_ctx);
+        ctx = dev->ctx[new_ctx];
+        mfc_debug(1, "Seting new context to %p\n", ctx);
+        /* Got context to run in ctx */
+        mfc_debug(1, "ctx->dst_queue_cnt=%d ctx->dpb_count=%d ctx->src_queue_cnt=%d\n",
+                ctx->dst_queue_cnt, ctx->dpb_count, ctx->src_queue_cnt);
+        mfc_debug(1, "ctx->state=%d\n", ctx->state);
+        /* Last frame has already been sent to MFC
+         * Now obtaining frames from MFC buffer */
+
+        s5p_mfc_clock_on();
+        if (ctx->type == MFCINST_DECODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                        s5p_mfc_run_dec_last_frames(ctx);
+                        break;
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_dec_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED:
+                        ret = s5p_mfc_run_init_dec_buffers(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_INIT:
+                        s5p_mfc_run_dec_last_frames(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_FLUSH:
+                        s5p_mfc_run_dec_last_frames(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_END:
+                        mfc_debug(2, "Finished remaining frames after resolution change.\n");
+                        ctx->capture_state = QUEUE_FREE;
+                        mfc_debug(2, "Will re-init the codec`.\n");
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else if (ctx->type == MFCINST_ENCODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_enc_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_enc(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED: /* Only for MFC6.x */
+                        ret = s5p_mfc_run_init_enc_buffers(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else {
+                mfc_err("invalid context type: %d\n", ctx->type);
+                ret = -EAGAIN;
+        }
+
+        if (ret) {
+                /* Free hardware lock */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        mfc_err("Failed to unlock hardware.\n");
+
+                /* This is in deed imporant, as no operation has been
+                 * scheduled, reduce the clock count as no one will
+                 * ever do this, because no interrupt related to this try_run
+                 * will ever come from hardware. */
+                s5p_mfc_clock_off();
+        }
+}
+
+
+void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
+{
+        struct s5p_mfc_buf *b;
+        int i;
+
+        while (!list_empty(lh)) {
+                b = list_entry(lh->next, struct s5p_mfc_buf, list);
+                for (i = 0; i < b->b->num_planes; i++)
+                        vb2_set_plane_payload(b->b, i, 0);
+                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+                list_del(&b->list);
+        }
+}
+
+void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_INT_V6);
+}
+
+void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
+                unsigned int ofs)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_clock_on();
+        WRITEL(data, ofs);
+        s5p_mfc_clock_off();
+}
+
+unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int ret;
+
+        s5p_mfc_clock_on();
+        ret = READL(ofs);
+        s5p_mfc_clock_off();
+
+        return ret;
+}
+
+int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+}
+
+int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+}
+
+int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_STATUS_V6);
+}
+
+int s5p_mfc_get_decoded_status_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DECODED_STATUS_V6);
+}
+
+int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DECODED_FRAME_TYPE_V6) &
+                S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
+{
+        return mfc_read(ctx->dev, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6) &
+                S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
+}
+
+int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RISC2HOST_CMD_V6) &
+                S5P_FIMV_RISC2HOST_CMD_MASK;
+}
+
+int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ERROR_CODE_V6);
+}
+
+int s5p_mfc_err_dec_v6(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DEC_MASK_V6) >> S5P_FIMV_ERR_DEC_SHIFT_V6;
+}
+
+int s5p_mfc_err_dspl_v6(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DSPL_MASK_V6) >> S5P_FIMV_ERR_DSPL_SHIFT_V6;
+}
+
+int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
+}
+
+int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
+}
+
+int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MIN_NUM_DPB_V6);
+}
+
+int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MIN_NUM_MV_V6);
+}
+
+int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RET_INSTANCE_ID_V6);
+}
+
+int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_NUM_DPB_V6);
+}
+
+int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_STREAM_SIZE_V6);
+}
+
+int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_SLICE_TYPE_V6);
+}
+
+int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_PICTURE_COUNT_V6);
+}
+
+int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
+{
+        return mfc_read(ctx->dev, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
+}
+
+int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
+}
+
+int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MVC_VIEW_ID_V6);
+}
+
+unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, PIC_TIME_TOP_V6);
+}
+
+unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, PIC_TIME_BOT_V6);
+}
+
+unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, CROP_INFO_H_V6);
+}
+
+unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, CROP_INFO_V_V6);
+}
+
+/* Initialize opr function pointers for MFC v6 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
+        .alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v6,
+        .release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v6,
+        .alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v6,
+        .release_codec_buffers = s5p_mfc_release_codec_buffers_v6,
+        .alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v6,
+        .release_instance_buffer = s5p_mfc_release_instance_buffer_v6,
+        .alloc_dev_context_buffer =
+                s5p_mfc_alloc_dev_context_buffer_v6,
+        .release_dev_context_buffer =
+                s5p_mfc_release_dev_context_buffer_v6,
+        .dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v6,
+        .enc_calc_src_size = s5p_mfc_enc_calc_src_size_v6,
+        .set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v6,
+        .set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v6,
+        .set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v6,
+        .set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v6,
+        .get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v6,
+        .set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v6,
+        .init_decode = s5p_mfc_init_decode_v6,
+        .init_encode = s5p_mfc_init_encode_v6,
+        .encode_one_frame = s5p_mfc_encode_one_frame_v6,
+        .try_run = s5p_mfc_try_run_v6,
+        .cleanup_queue = s5p_mfc_cleanup_queue_v6,
+        .clear_int_flags = s5p_mfc_clear_int_flags_v6,
+        .write_info = s5p_mfc_write_info_v6,
+        .read_info = s5p_mfc_read_info_v6,
+        .get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v6,
+        .get_dec_y_adr = s5p_mfc_get_dec_y_adr_v6,
+        .get_dspl_status = s5p_mfc_get_dspl_status_v6,
+        .get_dec_status = s5p_mfc_get_dec_status_v6,
+        .get_dec_frame_type = s5p_mfc_get_dec_frame_type_v6,
+        .get_disp_frame_type = s5p_mfc_get_disp_frame_type_v6,
+        .get_consumed_stream = s5p_mfc_get_consumed_stream_v6,
+        .get_int_reason = s5p_mfc_get_int_reason_v6,
+        .get_int_err = s5p_mfc_get_int_err_v6,
+        .err_dec = s5p_mfc_err_dec_v6,
+        .err_dspl = s5p_mfc_err_dspl_v6,
+        .get_img_width = s5p_mfc_get_img_width_v6,
+        .get_img_height = s5p_mfc_get_img_height_v6,
+        .get_dpb_count = s5p_mfc_get_dpb_count_v6,
+        .get_mv_count = s5p_mfc_get_mv_count_v6,
+        .get_inst_no = s5p_mfc_get_inst_no_v6,
+        .get_enc_strm_size = s5p_mfc_get_enc_strm_size_v6,
+        .get_enc_slice_type = s5p_mfc_get_enc_slice_type_v6,
+        .get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v6,
+        .get_enc_pic_count = s5p_mfc_get_enc_pic_count_v6,
+        .get_sei_avail_status = s5p_mfc_get_sei_avail_status_v6,
+        .get_mvc_num_views = s5p_mfc_get_mvc_num_views_v6,
+        .get_mvc_view_id = s5p_mfc_get_mvc_view_id_v6,
+        .get_pic_type_top = s5p_mfc_get_pic_type_top_v6,
+        .get_pic_type_bot = s5p_mfc_get_pic_type_bot_v6,
+        .get_crop_info_h = s5p_mfc_get_crop_info_h_v6,
+        .get_crop_info_v = s5p_mfc_get_crop_info_v_v6,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void)
+{
+        return &s5p_mfc_ops_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
new file mode 100644
index 000000000000..ab164efa127e
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
@@ -0,0 +1,50 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_OPR_V6_H_
+#define S5P_MFC_OPR_V6_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
+
+#define MFC_CTRL_MODE_CUSTOM    MFC_CTRL_MODE_SFR
+
+#define MB_WIDTH(x_size)                DIV_ROUND_UP(x_size, 16)
+#define MB_HEIGHT(y_size)               DIV_ROUND_UP(y_size, 16)
+#define S5P_MFC_DEC_MV_SIZE_V6(x, y)    (MB_WIDTH(x) * \
+                                        (((MB_HEIGHT(y)+1)/2)*2) * 64 + 128)
+
+/* Definition */
+#define ENC_MULTI_SLICE_MB_MAX          ((1 << 30) - 1)
+#define ENC_MULTI_SLICE_BIT_MIN         2800
+#define ENC_INTRA_REFRESH_MB_MAX        ((1 << 18) - 1)
+#define ENC_VBV_BUF_SIZE_MAX            ((1 << 30) - 1)
+#define ENC_H264_LOOP_FILTER_AB_MIN     -12
+#define ENC_H264_LOOP_FILTER_AB_MAX     12
+#define ENC_H264_RC_FRAME_RATE_MAX      ((1 << 16) - 1)
+#define ENC_H263_RC_FRAME_RATE_MAX      ((1 << 16) - 1)
+#define ENC_H264_PROFILE_MAX            3
+#define ENC_H264_LEVEL_MAX              42
+#define ENC_MPEG4_VOP_TIME_RES_MAX      ((1 << 16) - 1)
+#define FRAME_DELTA_H264_H263           1
+#define TIGHT_CBR_MAX                   10
+
+/* Definitions for shared memory compatibility */
+#define PIC_TIME_TOP_V6         S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6
+#define PIC_TIME_BOT_V6         S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6
+#define CROP_INFO_H_V6          S5P_FIMV_D_DISPLAY_CROP_INFO1_V6
+#define CROP_INFO_V_V6          S5P_FIMV_D_DISPLAY_CROP_INFO2_V6
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void);
+#endif /* S5P_MFC_OPR_V6_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
index 0503d14ac94e..367db7552289 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
@@ -20,7 +20,6 @@
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_pm.h"
 
-#define MFC_CLKNAME             "sclk_mfc"
 #define MFC_GATE_CLK_NAME       "mfc"
 
 #define CLK_DEBUG
@@ -51,7 +50,7 @@ int s5p_mfc_init_pm(struct s5p_mfc_dev *dev)
                 goto err_p_ip_clk;
         }
 
-        pm->clock = clk_get(&dev->plat_dev->dev, MFC_CLKNAME);
+        pm->clock = clk_get(&dev->plat_dev->dev, dev->variant->mclk_name);
         if (IS_ERR(pm->clock)) {
                 mfc_err("Failed to get MFC clock\n");
                 ret = PTR_ERR(pm->clock);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
deleted file mode 100644
index b5933d233a4b..000000000000
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * linux/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
- *
- * Copyright (c) 2010 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifdef CONFIG_ARCH_EXYNOS4
-#include <linux/dma-mapping.h>
-#endif
-#include <linux/io.h>
-#include "s5p_mfc_common.h"
-#include "s5p_mfc_debug.h"
-
-int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        void *shm_alloc_ctx = dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
-
-        ctx->shm_alloc = vb2_dma_contig_memops.alloc(shm_alloc_ctx,
-                                                        SHARED_BUF_SIZE);
-        if (IS_ERR(ctx->shm_alloc)) {
-                mfc_err("failed to allocate shared memory\n");
-                return PTR_ERR(ctx->shm_alloc);
-        }
-        /* shm_ofs only keeps the offset from base (port a) */
-        ctx->shm_ofs = s5p_mfc_mem_cookie(shm_alloc_ctx, ctx->shm_alloc)
-                                                                - dev->bank1;
-        BUG_ON(ctx->shm_ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        ctx->shm = vb2_dma_contig_memops.vaddr(ctx->shm_alloc);
-        if (!ctx->shm) {
-                vb2_dma_contig_memops.put(ctx->shm_alloc);
-                ctx->shm_ofs = 0;
-                ctx->shm_alloc = NULL;
-                mfc_err("failed to virt addr of shared memory\n");
-                return -ENOMEM;
-        }
-        memset((void *)ctx->shm, 0, SHARED_BUF_SIZE);
-        wmb();
-        return 0;
-}
-
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 403d7f17bfab..9fd9d1c5b218 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -1376,6 +1376,7 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
         __u32 pixfmt = pix->pixelformat;
         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
+        struct mx2_fmt_cfg *emma_prp;
         unsigned int width_limit;
         int ret;
 
@@ -1438,12 +1439,11 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
                 __func__, pcdev->s_width, pcdev->s_height);
 
         /* If the sensor does not support image size try PrP resizing */
-        pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
+        emma_prp = mx27_emma_prp_get_format(xlate->code,
                                                    xlate->host_fmt->fourcc);
 
-        memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
         if ((mf.width != pix->width || mf.height != pix->height) &&
-                pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
+                emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
                 if (mx2_emmaprp_resize(pcdev, &mf, pix, false) < 0)
                         dev_dbg(icd->parent, "%s: can't resize\n", __func__);
         }
@@ -1655,6 +1655,7 @@ static int __devinit mx27_camera_emma_init(struct platform_device *pdev)
         irq_emma = platform_get_irq(pdev, 1);
         if (!res_emma || !irq_emma) {
                 dev_err(pcdev->dev, "no EMMA resources\n");
+                err = -ENODEV;
                 goto out;
         }
 
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 3be92944f8e7..d3f0b84e2d70 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -950,11 +950,11 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
         /* In all these cases cropping emulation will not help */
         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-            (s->target != V4L2_SEL_TGT_COMPOSE_ACTIVE &&
-             s->target != V4L2_SEL_TGT_CROP_ACTIVE))
+            (s->target != V4L2_SEL_TGT_COMPOSE &&
+             s->target != V4L2_SEL_TGT_CROP))
                 return -EINVAL;
 
-        if (s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+        if (s->target == V4L2_SEL_TGT_COMPOSE) {
                 /* No output size change during a running capture! */
                 if (is_streaming(ici, icd) &&
                     (icd->user_width != s->r.width ||
@@ -974,7 +974,7 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
         ret = ici->ops->set_selection(icd, s);
         if (!ret &&
-            s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+            s->target == V4L2_SEL_TGT_COMPOSE) {
                 icd->user_width = s->r.width;
                 icd->user_height = s->r.height;
                 if (!icd->streamer)
@@ -1184,7 +1184,8 @@ static int soc_camera_probe(struct soc_camera_device *icd)
         sd->grp_id = soc_camera_grp_id(icd);
         v4l2_set_subdev_hostdata(sd, icd);
 
-        if (v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL))
+        ret = v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL);
+        if (ret < 0)
                 goto ectrl;
 
         /* At this point client .probe() should have run already */
@@ -1529,12 +1530,11 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
 {
         struct soc_camera_link *icl = pdev->dev.platform_data;
         struct soc_camera_device *icd;
-        int ret;
 
         if (!icl)
                 return -EINVAL;
 
-        icd = kzalloc(sizeof(*icd), GFP_KERNEL);
+        icd = devm_kzalloc(&pdev->dev, sizeof(*icd), GFP_KERNEL);
         if (!icd)
                 return -ENOMEM;
 
@@ -1543,19 +1543,10 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
         icd->pdev = &pdev->dev;
         platform_set_drvdata(pdev, icd);
 
-        ret = soc_camera_device_register(icd);
-        if (ret < 0)
-                goto escdevreg;
-
         icd->user_width         = DEFAULT_WIDTH;
         icd->user_height        = DEFAULT_HEIGHT;
 
-        return 0;
-
-escdevreg:
-        kfree(icd);
-
-        return ret;
+        return soc_camera_device_register(icd);
 }
 
 /*
@@ -1572,8 +1563,6 @@ static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
 
         list_del(&icd->list);
 
-        kfree(icd);
-
         return 0;
 }
 
@@ -1586,18 +1575,7 @@ static struct platform_driver __refdata soc_camera_pdrv = {
         },
 };
 
-static int __init soc_camera_init(void)
-{
-        return platform_driver_register(&soc_camera_pdrv);
-}
-
-static void __exit soc_camera_exit(void)
-{
-        platform_driver_unregister(&soc_camera_pdrv);
-}
-
-module_init(soc_camera_init);
-module_exit(soc_camera_exit);
+module_platform_driver(soc_camera_pdrv);
 
 MODULE_DESCRIPTION("Image capture bus driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index e5024cfd27a7..4ef55ec8045e 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -308,7 +308,7 @@ static irqreturn_t si470x_i2c_interrupt(int irq, void *dev_id)
                                         READCHAN_BLERD) >> 10;
                         rds = radio->registers[RDSD];
                         break;
-                };
+                }
 
                 /* Fill the V4L2 RDS buffer */
                 put_unaligned_le16(rds, &tmpbuf);
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index be076f7181e7..62f3edec39bc 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -446,7 +446,7 @@ static void si470x_int_in_callback(struct urb *urb)
                                                 READCHAN_BLERD) >> 10;
                                 rds = radio->registers[RDSD];
                                 break;
-                        };
+                        }
 
                         /* Fill the V4L2 RDS buffer */
                         put_unaligned_le16(rds, &tmpbuf);
diff --git a/drivers/media/radio/si4713-i2c.c b/drivers/media/radio/si4713-i2c.c
index a9e6d17015ef..e3079c142c5f 100644
--- a/drivers/media/radio/si4713-i2c.c
+++ b/drivers/media/radio/si4713-i2c.c
@@ -1009,7 +1009,7 @@ static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
 
         default:
                 rval = -EINVAL;
-        };
+        }
 
         return rval;
 }
@@ -1081,7 +1081,7 @@ static int si4713_write_econtrol_string(struct si4713_device *sdev,
         default:
                 rval = -EINVAL;
                 break;
-        };
+        }
 
 exit:
         return rval;
@@ -1130,7 +1130,7 @@ static int si4713_write_econtrol_tune(struct si4713_device *sdev,
         default:
                 rval = -EINVAL;
                 goto unlock;
-        };
+        }
 
         if (sdev->power_state)
                 rval = si4713_tx_tune_power(sdev, power, antcap);
@@ -1420,7 +1420,7 @@ static int si4713_read_econtrol_string(struct si4713_device *sdev,
         default:
                 rval = -EINVAL;
                 break;
-        };
+        }
 
 exit:
         return rval;
@@ -1473,7 +1473,7 @@ static int si4713_read_econtrol_tune(struct si4713_device *sdev,
                 break;
         default:
                 rval = -EINVAL;
-        };
+        }
 
 unlock:
         mutex_unlock(&sdev->mutex);
@@ -1698,7 +1698,7 @@ static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
         default:
                 rval = -EINVAL;
                 break;
-        };
+        }
 
         return rval;
 }
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index 647dd951b0e8..d05ac15b5de4 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -881,10 +881,13 @@ static int ene_set_tx_mask(struct rc_dev *rdev, u32 tx_mask)
 static int ene_set_tx_carrier(struct rc_dev *rdev, u32 carrier)
 {
         struct ene_device *dev = rdev->priv;
-        u32 period = 2000000 / carrier;
+        u32 period;
 
         dbg("TX: attempt to set tx carrier to %d kHz", carrier);
+        if (carrier == 0)
+                return -EINVAL;
 
+        period = 2000000 / carrier;
         if (period && (period > ENE_CIRMOD_PRD_MAX ||
                         period < ENE_CIRMOD_PRD_MIN)) {
 
diff --git a/drivers/media/rc/iguanair.c b/drivers/media/rc/iguanair.c
index 1e4c68a5cecf..51d7057aca04 100644
--- a/drivers/media/rc/iguanair.c
+++ b/drivers/media/rc/iguanair.c
@@ -28,6 +28,7 @@
 #include <media/rc-core.h>
 
 #define DRIVER_NAME "iguanair"
+#define BUF_SIZE 152
 
 struct iguanair {
         struct rc_dev *rc;
@@ -35,26 +36,23 @@ struct iguanair {
         struct device *dev;
         struct usb_device *udev;
 
-        int pipe_out;
         uint16_t version;
         uint8_t bufsize;
+        uint8_t cycle_overhead;
 
         struct mutex lock;
 
         /* receiver support */
         bool receiver_on;
-        dma_addr_t dma_in;
+        dma_addr_t dma_in, dma_out;
         uint8_t *buf_in;
-        struct urb *urb_in;
+        struct urb *urb_in, *urb_out;
         struct completion completion;
 
         /* transmit support */
         bool tx_overflow;
         uint32_t carrier;
-        uint8_t cycle_overhead;
-        uint8_t channels;
-        uint8_t busy4;
-        uint8_t busy7;
+        struct send_packet *packet;
 
         char name[64];
         char phys[64];
@@ -73,7 +71,8 @@ struct iguanair {
 #define DIR_IN                  0xdc
 #define DIR_OUT                 0xcd
 
-#define MAX_PACKET_SIZE         8u
+#define MAX_IN_PACKET           8u
+#define MAX_OUT_PACKET          (sizeof(struct send_packet) + BUF_SIZE)
 #define TIMEOUT                 1000
 #define RX_RESOLUTION           21333
 
@@ -191,20 +190,25 @@ static void iguanair_rx(struct urb *urb)
                 dev_warn(ir->dev, "failed to resubmit urb: %d\n", rc);
 }
 
-static int iguanair_send(struct iguanair *ir, void *data, unsigned size)
+static void iguanair_irq_out(struct urb *urb)
 {
-        int rc, transferred;
+        struct iguanair *ir = urb->context;
+
+        if (urb->status)
+                dev_dbg(ir->dev, "Error: out urb status = %d\n", urb->status);
+}
+
+static int iguanair_send(struct iguanair *ir, unsigned size)
+{
+        int rc;
 
         INIT_COMPLETION(ir->completion);
 
-        rc = usb_interrupt_msg(ir->udev, ir->pipe_out, data, size,
-                                                        &transferred, TIMEOUT);
+        ir->urb_out->transfer_buffer_length = size;
+        rc = usb_submit_urb(ir->urb_out, GFP_KERNEL);
         if (rc)
                 return rc;
 
-        if (transferred != size)
-                return -EIO;
-
         if (wait_for_completion_timeout(&ir->completion, TIMEOUT) == 0)
                 return -ETIMEDOUT;
 
@@ -213,14 +217,13 @@ static int iguanair_send(struct iguanair *ir, void *data, unsigned size)
 
 static int iguanair_get_features(struct iguanair *ir)
 {
-        struct packet packet;
         int rc;
 
-        packet.start = 0;
-        packet.direction = DIR_OUT;
-        packet.cmd = CMD_GET_VERSION;
+        ir->packet->header.start = 0;
+        ir->packet->header.direction = DIR_OUT;
+        ir->packet->header.cmd = CMD_GET_VERSION;
 
-        rc = iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
         if (rc) {
                 dev_info(ir->dev, "failed to get version\n");
                 goto out;
@@ -235,17 +238,23 @@ static int iguanair_get_features(struct iguanair *ir)
         ir->bufsize = 150;
         ir->cycle_overhead = 65;
 
-        packet.cmd = CMD_GET_BUFSIZE;
+        ir->packet->header.cmd = CMD_GET_BUFSIZE;
 
-        rc = iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
         if (rc) {
                 dev_info(ir->dev, "failed to get buffer size\n");
                 goto out;
         }
 
-        packet.cmd = CMD_GET_FEATURES;
+        if (ir->bufsize > BUF_SIZE) {
+                dev_info(ir->dev, "buffer size %u larger than expected\n",
+                                                                ir->bufsize);
+                ir->bufsize = BUF_SIZE;
+        }
+
+        ir->packet->header.cmd = CMD_GET_FEATURES;
 
-        rc = iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
         if (rc) {
                 dev_info(ir->dev, "failed to get features\n");
                 goto out;
@@ -257,13 +266,18 @@ out:
 
 static int iguanair_receiver(struct iguanair *ir, bool enable)
 {
-        struct packet packet = { 0, DIR_OUT, enable ?
-                                CMD_RECEIVER_ON : CMD_RECEIVER_OFF };
+        int rc;
+
+        ir->packet->header.start = 0;
+        ir->packet->header.direction = DIR_OUT;
+        ir->packet->header.cmd = enable ? CMD_RECEIVER_ON : CMD_RECEIVER_OFF;
 
         if (enable)
                 ir_raw_event_reset(ir->rc);
 
-        return iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
+
+        return rc;
 }
 
 /*
@@ -308,8 +322,8 @@ static int iguanair_set_tx_carrier(struct rc_dev *dev, uint32_t carrier)
                 fours = (cycles - sevens * 7) / 4;
 
                 /* magic happens here */
-                ir->busy7 = (4 - sevens) * 2;
-                ir->busy4 = 110 - fours;
+                ir->packet->busy7 = (4 - sevens) * 2;
+                ir->packet->busy4 = 110 - fours;
         }
 
         mutex_unlock(&ir->lock);
@@ -325,7 +339,7 @@ static int iguanair_set_tx_mask(struct rc_dev *dev, uint32_t mask)
                 return 4;
 
         mutex_lock(&ir->lock);
-        ir->channels = mask;
+        ir->packet->channels = mask << 4;
         mutex_unlock(&ir->lock);
 
         return 0;
@@ -337,16 +351,9 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
         uint8_t space;
         unsigned i, size, periods, bytes;
         int rc;
-        struct send_packet *packet;
 
         mutex_lock(&ir->lock);
 
-        packet = kmalloc(sizeof(*packet) + ir->bufsize, GFP_KERNEL);
-        if (!packet) {
-                rc = -ENOMEM;
-                goto out;
-        }
-
         /* convert from us to carrier periods */
         for (i = space = size = 0; i < count; i++) {
                 periods = DIV_ROUND_CLOSEST(txbuf[i] * ir->carrier, 1000000);
@@ -356,11 +363,11 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
                         break;
                 }
                 while (periods > 127) {
-                        packet->payload[size++] = 127 | space;
+                        ir->packet->payload[size++] = 127 | space;
                         periods -= 127;
                 }
 
-                packet->payload[size++] = periods | space;
+                ir->packet->payload[size++] = periods | space;
                 space ^= 0x80;
         }
 
@@ -369,36 +376,19 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
                 goto out;
         }
 
-        packet->header.start = 0;
-        packet->header.direction = DIR_OUT;
-        packet->header.cmd = CMD_SEND;
-        packet->length = size;
-        packet->channels = ir->channels << 4;
-        packet->busy7 = ir->busy7;
-        packet->busy4 = ir->busy4;
-
-        if (ir->receiver_on) {
-                rc = iguanair_receiver(ir, false);
-                if (rc) {
-                        dev_warn(ir->dev, "disable receiver before transmit failed\n");
-                        goto out;
-                }
-        }
+        ir->packet->header.start = 0;
+        ir->packet->header.direction = DIR_OUT;
+        ir->packet->header.cmd = CMD_SEND;
+        ir->packet->length = size;
 
         ir->tx_overflow = false;
 
-        rc = iguanair_send(ir, packet, size + 8);
+        rc = iguanair_send(ir, sizeof(*ir->packet) + size);
 
         if (rc == 0 && ir->tx_overflow)
                 rc = -EOVERFLOW;
 
-        if (ir->receiver_on) {
-                if (iguanair_receiver(ir, true))
-                        dev_warn(ir->dev, "re-enable receiver after transmit failed\n");
-        }
-
 out:
-        kfree(packet);
         mutex_unlock(&ir->lock);
 
         return rc ? rc : count;
@@ -411,8 +401,6 @@ static int iguanair_open(struct rc_dev *rdev)
 
         mutex_lock(&ir->lock);
 
-        BUG_ON(ir->receiver_on);
-
         rc = iguanair_receiver(ir, true);
         if (rc == 0)
                 ir->receiver_on = true;
@@ -443,7 +431,7 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
         struct usb_device *udev = interface_to_usbdev(intf);
         struct iguanair *ir;
         struct rc_dev *rc;
-        int ret, pipein;
+        int ret, pipein, pipeout;
         struct usb_host_interface *idesc;
 
         ir = kzalloc(sizeof(*ir), GFP_KERNEL);
@@ -453,11 +441,14 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
                 goto out;
         }
 
-        ir->buf_in = usb_alloc_coherent(udev, MAX_PACKET_SIZE, GFP_KERNEL,
+        ir->buf_in = usb_alloc_coherent(udev, MAX_IN_PACKET, GFP_KERNEL,
                                                                 &ir->dma_in);
+        ir->packet = usb_alloc_coherent(udev, MAX_OUT_PACKET, GFP_KERNEL,
+                                                                &ir->dma_out);
         ir->urb_in = usb_alloc_urb(0, GFP_KERNEL);
+        ir->urb_out = usb_alloc_urb(0, GFP_KERNEL);
 
-        if (!ir->buf_in || !ir->urb_in) {
+        if (!ir->buf_in || !ir->packet || !ir->urb_in || !ir->urb_out) {
                 ret = -ENOMEM;
                 goto out;
         }
@@ -472,13 +463,18 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
         ir->rc = rc;
         ir->dev = &intf->dev;
         ir->udev = udev;
-        ir->pipe_out = usb_sndintpipe(udev,
-                                idesc->endpoint[1].desc.bEndpointAddress);
         mutex_init(&ir->lock);
+
         init_completion(&ir->completion);
+        pipeout = usb_sndintpipe(udev,
+                                idesc->endpoint[1].desc.bEndpointAddress);
+        usb_fill_int_urb(ir->urb_out, udev, pipeout, ir->packet, MAX_OUT_PACKET,
+                                                iguanair_irq_out, ir, 1);
+        ir->urb_out->transfer_dma = ir->dma_out;
+        ir->urb_out->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
         pipein = usb_rcvintpipe(udev, idesc->endpoint[0].desc.bEndpointAddress);
-        usb_fill_int_urb(ir->urb_in, udev, pipein, ir->buf_in, MAX_PACKET_SIZE,
+        usb_fill_int_urb(ir->urb_in, udev, pipein, ir->buf_in, MAX_IN_PACKET,
                                                          iguanair_rx, ir, 1);
         ir->urb_in->transfer_dma = ir->dma_in;
         ir->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
@@ -528,11 +524,14 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
         return 0;
 out2:
         usb_kill_urb(ir->urb_in);
+        usb_kill_urb(ir->urb_out);
 out:
         if (ir) {
                 usb_free_urb(ir->urb_in);
-                usb_free_coherent(udev, MAX_PACKET_SIZE, ir->buf_in,
-                                                                ir->dma_in);
+                usb_free_urb(ir->urb_out);
+                usb_free_coherent(udev, MAX_IN_PACKET, ir->buf_in, ir->dma_in);
+                usb_free_coherent(udev, MAX_OUT_PACKET, ir->packet,
+                                                                ir->dma_out);
         }
         rc_free_device(rc);
         kfree(ir);
@@ -546,8 +545,11 @@ static void __devexit iguanair_disconnect(struct usb_interface *intf)
         rc_unregister_device(ir->rc);
         usb_set_intfdata(intf, NULL);
         usb_kill_urb(ir->urb_in);
+        usb_kill_urb(ir->urb_out);
         usb_free_urb(ir->urb_in);
-        usb_free_coherent(ir->udev, MAX_PACKET_SIZE, ir->buf_in, ir->dma_in);
+        usb_free_urb(ir->urb_out);
+        usb_free_coherent(ir->udev, MAX_IN_PACKET, ir->buf_in, ir->dma_in);
+        usb_free_coherent(ir->udev, MAX_OUT_PACKET, ir->packet, ir->dma_out);
         kfree(ir);
 }
 
@@ -565,6 +567,7 @@ static int iguanair_suspend(struct usb_interface *intf, pm_message_t message)
         }
 
         usb_kill_urb(ir->urb_in);
+        usb_kill_urb(ir->urb_out);
 
         mutex_unlock(&ir->lock);
 
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index 569124b03de3..870c93052fd0 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -203,13 +203,13 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
         /* TX settings */
         case LIRC_SET_TRANSMITTER_MASK:
                 if (!dev->s_tx_mask)
-                        return -EINVAL;
+                        return -ENOSYS;
 
                 return dev->s_tx_mask(dev, val);
 
         case LIRC_SET_SEND_CARRIER:
                 if (!dev->s_tx_carrier)
-                        return -EINVAL;
+                        return -ENOSYS;
 
                 return dev->s_tx_carrier(dev, val);
 
diff --git a/drivers/media/rc/keymaps/rc-msi-digivox-ii.c b/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
index c64e9e30045d..2fa71d0d72d7 100644
--- a/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
+++ b/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
@@ -22,24 +22,24 @@
 #include <linux/module.h>
 
 static struct rc_map_table msi_digivox_ii[] = {
-        { 0x0002, KEY_2 },
-        { 0x0003, KEY_UP },              /* up */
-        { 0x0004, KEY_3 },
-        { 0x0005, KEY_CHANNELDOWN },
-        { 0x0008, KEY_5 },
-        { 0x0009, KEY_0 },
-        { 0x000b, KEY_8 },
-        { 0x000d, KEY_DOWN },            /* down */
-        { 0x0010, KEY_9 },
-        { 0x0011, KEY_7 },
-        { 0x0014, KEY_VOLUMEUP },
-        { 0x0015, KEY_CHANNELUP },
-        { 0x0016, KEY_OK },
-        { 0x0017, KEY_POWER2 },
-        { 0x001a, KEY_1 },
-        { 0x001c, KEY_4 },
-        { 0x001d, KEY_6 },
-        { 0x001f, KEY_VOLUMEDOWN },
+        { 0x0302, KEY_2 },
+        { 0x0303, KEY_UP },              /* up */
+        { 0x0304, KEY_3 },
+        { 0x0305, KEY_CHANNELDOWN },
+        { 0x0308, KEY_5 },
+        { 0x0309, KEY_0 },
+        { 0x030b, KEY_8 },
+        { 0x030d, KEY_DOWN },            /* down */
+        { 0x0310, KEY_9 },
+        { 0x0311, KEY_7 },
+        { 0x0314, KEY_VOLUMEUP },
+        { 0x0315, KEY_CHANNELUP },
+        { 0x0316, KEY_OK },
+        { 0x0317, KEY_POWER2 },
+        { 0x031a, KEY_1 },
+        { 0x031c, KEY_4 },
+        { 0x031d, KEY_6 },
+        { 0x031f, KEY_VOLUMEDOWN },
 };
 
 static struct rc_map_list msi_digivox_ii_map = {
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 699eef39128b..2ea913a44ae8 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -517,6 +517,9 @@ static int nvt_set_tx_carrier(struct rc_dev *dev, u32 carrier)
         struct nvt_dev *nvt = dev->priv;
         u16 val;
 
+        if (carrier == 0)
+                return -EINVAL;
+
         nvt_cir_reg_write(nvt, 1, CIR_CP);
         val = 3000000 / (carrier) - 1;
         nvt_cir_reg_write(nvt, val & 0xff, CIR_CC);
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 49731b1a9c57..9f5a17bb5ef5 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -890,6 +890,9 @@ static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
         struct device *dev = rr3->dev;
 
         rr3_dbg(dev, "Setting modulation frequency to %u", carrier);
+        if (carrier == 0)
+                return -EINVAL;
+
         rr3->carrier = carrier;
 
         return carrier;
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index 30ae1f24abc3..7c9b5f33113b 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -184,7 +184,7 @@ enum wbcir_txstate {
 };
 
 /* Misc */
-#define WBCIR_NAME      "Winbond CIR"
+#define WBCIR_NAME      "winbond-cir"
 #define WBCIR_ID_FAMILY          0xF1 /* Family ID for the WPCD376I     */
 #define WBCIR_ID_CHIP            0x04 /* Chip ID for the WPCD376I       */
 #define INVALID_SCANCODE   0x7FFFFFFF /* Invalid with all protos        */
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index 0ed9091ff48e..2e1a02e360ff 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -245,7 +245,7 @@ struct mt2063_state {
 /*
  * mt2063_write - Write data into the I2C bus
  */
-static u32 mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
+static int mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
 {
         struct dvb_frontend *fe = state->frontend;
         int ret;
@@ -277,9 +277,9 @@ static u32 mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
 /*
  * mt2063_write - Write register data into the I2C bus, caching the value
  */
-static u32 mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
+static int mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
 {
-        u32 status;
+        int status;
 
         dprintk(2, "\n");
 
@@ -298,10 +298,10 @@ static u32 mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
 /*
  * mt2063_read - Read data from the I2C bus
  */
-static u32 mt2063_read(struct mt2063_state *state,
+static int mt2063_read(struct mt2063_state *state,
                            u8 subAddress, u8 *pData, u32 cnt)
 {
-        u32 status = 0; /* Status to be returned        */
+        int status = 0; /* Status to be returned        */
         struct dvb_frontend *fe = state->frontend;
         u32 i = 0;
 
@@ -816,7 +816,7 @@ static u32 IsSpurInBand(struct MT2063_AvoidSpursData_t *pAS_Info,
  */
 static u32 MT2063_AvoidSpurs(struct MT2063_AvoidSpursData_t *pAS_Info)
 {
-        u32 status = 0;
+        int status = 0;
         u32 fm, fp;             /*  restricted range on LO's        */
         pAS_Info->bSpurAvoided = 0;
         pAS_Info->nSpursFound = 0;
@@ -935,14 +935,14 @@ static u32 MT2063_AvoidSpurs(struct MT2063_AvoidSpursData_t *pAS_Info)
  *
  * This function returns 0, if no lock, 1 if locked and a value < 1 if error
  */
-static unsigned int mt2063_lockStatus(struct mt2063_state *state)
+static int mt2063_lockStatus(struct mt2063_state *state)
 {
         const u32 nMaxWait = 100;       /*  wait a maximum of 100 msec   */
         const u32 nPollRate = 2;        /*  poll status bits every 2 ms */
         const u32 nMaxLoops = nMaxWait / nPollRate;
         const u8 LO1LK = 0x80;
         u8 LO2LK = 0x08;
-        u32 status;
+        int status;
         u32 nDelays = 0;
 
         dprintk(2, "\n");
@@ -1069,7 +1069,7 @@ static u32 mt2063_get_dnc_output_enable(struct mt2063_state *state,
 static u32 mt2063_set_dnc_output_enable(struct mt2063_state *state,
                                         enum MT2063_DNC_Output_Enable nValue)
 {
-        u32 status = 0; /* Status to be returned        */
+        int status = 0; /* Status to be returned        */
         u8 val = 0;
 
         dprintk(2, "\n");
@@ -1203,7 +1203,7 @@ static u32 mt2063_set_dnc_output_enable(struct mt2063_state *state,
 static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
                                   enum mt2063_delivery_sys Mode)
 {
-        u32 status = 0; /* Status to be returned        */
+        int status = 0; /* Status to be returned        */
         u8 val;
         u32 longval;
 
@@ -1345,7 +1345,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 static u32 MT2063_ClearPowerMaskBits(struct mt2063_state *state,
                                      enum MT2063_Mask_Bits Bits)
 {
-        u32 status = 0;
+        int status = 0;
 
         dprintk(2, "\n");
         Bits = (enum MT2063_Mask_Bits)(Bits & MT2063_ALL_SD);   /* Only valid bits for this tuner */
@@ -1374,7 +1374,7 @@ static u32 MT2063_ClearPowerMaskBits(struct mt2063_state *state,
  */
 static u32 MT2063_SoftwareShutdown(struct mt2063_state *state, u8 Shutdown)
 {
-        u32 status;
+        int status;
 
         dprintk(2, "\n");
         if (Shutdown == 1)
@@ -1540,7 +1540,7 @@ static u32 FindClearTuneFilter(struct mt2063_state *state, u32 f_in)
 static u32 MT2063_Tune(struct mt2063_state *state, u32 f_in)
 {                               /* RF input center frequency   */
 
-        u32 status = 0;
+        int status = 0;
         u32 LO1;                /*  1st LO register value           */
         u32 Num1;               /*  Numerator for LO1 reg. value    */
         u32 f_IF1;              /*  1st IF requested                */
@@ -1803,7 +1803,7 @@ static const u8 MT2063B3_defaults[] = {
 
 static int mt2063_init(struct dvb_frontend *fe)
 {
-        u32 status;
+        int status;
         struct mt2063_state *state = fe->tuner_priv;
         u8 all_resets = 0xF0;   /* reset/load bits */
         const u8 *def = NULL;
@@ -2249,8 +2249,8 @@ struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
         dprintk(2, "\n");
 
         state = kzalloc(sizeof(struct mt2063_state), GFP_KERNEL);
-        if (state == NULL)
-                goto error;
+        if (!state)
+                return NULL;
 
         state->config = config;
         state->i2c = i2c;
@@ -2261,18 +2261,15 @@ struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
 
         printk(KERN_INFO "%s: Attaching MT2063\n", __func__);
         return fe;
-
-error:
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL_GPL(mt2063_attach);
 
+#if 0
 /*
  * Ancillary routines visible outside mt2063
  * FIXME: Remove them in favor of using standard tuner callbacks
  */
-unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
+static int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
 {
         struct mt2063_state *state = fe->tuner_priv;
         int err = 0;
@@ -2285,9 +2282,8 @@ unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
 
         return err;
 }
-EXPORT_SYMBOL_GPL(tuner_MT2063_SoftwareShutdown);
 
-unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
+static int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
 {
         struct mt2063_state *state = fe->tuner_priv;
         int err = 0;
@@ -2300,7 +2296,7 @@ unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
 
         return err;
 }
-EXPORT_SYMBOL_GPL(tuner_MT2063_ClearPowerMaskBits);
+#endif
 
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
 MODULE_DESCRIPTION("MT2063 Silicon tuner");
diff --git a/drivers/media/tuners/mt2063.h b/drivers/media/tuners/mt2063.h
index 3f5cfd93713f..ab24170c1571 100644
--- a/drivers/media/tuners/mt2063.h
+++ b/drivers/media/tuners/mt2063.h
@@ -23,10 +23,6 @@ static inline struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
         return NULL;
 }
 
-/* FIXME: Should use the standard DVB attachment interfaces */
-unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe);
-unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe);
-
 #endif /* CONFIG_DVB_MT2063 */
 
 #endif /* __MT2063_H__ */
diff --git a/drivers/media/tuners/tda18271-common.c b/drivers/media/tuners/tda18271-common.c
index 221171eeb0c3..18c77afe2e4f 100644
--- a/drivers/media/tuners/tda18271-common.c
+++ b/drivers/media/tuners/tda18271-common.c
@@ -187,7 +187,8 @@ int tda18271_read_extended(struct dvb_frontend *fe)
         return (ret == 2 ? 0 : ret);
 }
 
-int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
+static int __tda18271_write_regs(struct dvb_frontend *fe, int idx, int len,
+                        bool lock_i2c)
 {
         struct tda18271_priv *priv = fe->tuner_priv;
         unsigned char *regs = priv->tda18271_regs;
@@ -198,7 +199,6 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
 
         BUG_ON((len == 0) || (idx + len > sizeof(buf)));
 
-
         switch (priv->small_i2c) {
         case TDA18271_03_BYTE_CHUNK_INIT:
                 max = 3;
@@ -214,7 +214,19 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
                 max = 39;
         }
 
-        tda18271_i2c_gate_ctrl(fe, 1);
+
+        /*
+         * If lock_i2c is true, it will take the I2C bus for tda18271 private
+         * usage during the entire write ops, as otherwise, bad things could
+         * happen.
+         * During device init, several write operations will happen. So,
+         * tda18271_init_regs controls the I2C lock directly,
+         * disabling lock_i2c here.
+         */
+        if (lock_i2c) {
+                tda18271_i2c_gate_ctrl(fe, 1);
+                i2c_lock_adapter(priv->i2c_props.adap);
+        }
         while (len) {
                 if (max > len)
                         max = len;
@@ -226,14 +238,17 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
                 msg.len = max + 1;
 
                 /* write registers */
-                ret = i2c_transfer(priv->i2c_props.adap, &msg, 1);
+                ret = __i2c_transfer(priv->i2c_props.adap, &msg, 1);
                 if (ret != 1)
                         break;
 
                 idx += max;
                 len -= max;
         }
-        tda18271_i2c_gate_ctrl(fe, 0);
+        if (lock_i2c) {
+                i2c_unlock_adapter(priv->i2c_props.adap);
+                tda18271_i2c_gate_ctrl(fe, 0);
+        }
 
         if (ret != 1)
                 tda_err("ERROR: idx = 0x%x, len = %d, "
@@ -242,10 +257,16 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
         return (ret == 1 ? 0 : ret);
 }
 
+int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
+{
+        return __tda18271_write_regs(fe, idx, len, true);
+}
+
 /*---------------------------------------------------------------------*/
 
-int tda18271_charge_pump_source(struct dvb_frontend *fe,
-                                enum tda18271_pll pll, int force)
+static int __tda18271_charge_pump_source(struct dvb_frontend *fe,
+                                         enum tda18271_pll pll, int force,
+                                         bool lock_i2c)
 {
         struct tda18271_priv *priv = fe->tuner_priv;
         unsigned char *regs = priv->tda18271_regs;
@@ -255,9 +276,16 @@ int tda18271_charge_pump_source(struct dvb_frontend *fe,
         regs[r_cp] &= ~0x20;
         regs[r_cp] |= ((force & 1) << 5);
 
-        return tda18271_write_regs(fe, r_cp, 1);
+        return __tda18271_write_regs(fe, r_cp, 1, lock_i2c);
+}
+
+int tda18271_charge_pump_source(struct dvb_frontend *fe,
+                                enum tda18271_pll pll, int force)
+{
+        return __tda18271_charge_pump_source(fe, pll, force, true);
 }
 
+
 int tda18271_init_regs(struct dvb_frontend *fe)
 {
         struct tda18271_priv *priv = fe->tuner_priv;
@@ -267,6 +295,13 @@ int tda18271_init_regs(struct dvb_frontend *fe)
                 i2c_adapter_id(priv->i2c_props.adap),
                 priv->i2c_props.addr);
 
+        /*
+         * Don't let any other I2C transfer to happen at adapter during init,
+         * as those could cause bad things
+         */
+        tda18271_i2c_gate_ctrl(fe, 1);
+        i2c_lock_adapter(priv->i2c_props.adap);
+
         /* initialize registers */
         switch (priv->id) {
         case TDA18271HDC1:
@@ -352,28 +387,28 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_EB22] = 0x48;
         regs[R_EB23] = 0xb0;
 
-        tda18271_write_regs(fe, 0x00, TDA18271_NUM_REGS);
+        __tda18271_write_regs(fe, 0x00, TDA18271_NUM_REGS, false);
 
         /* setup agc1 gain */
         regs[R_EB17] = 0x00;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
         regs[R_EB17] = 0x03;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
         regs[R_EB17] = 0x43;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
         regs[R_EB17] = 0x4c;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
 
         /* setup agc2 gain */
         if ((priv->id) == TDA18271HDC1) {
                 regs[R_EB20] = 0xa0;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
                 regs[R_EB20] = 0xa7;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
                 regs[R_EB20] = 0xe7;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
                 regs[R_EB20] = 0xec;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
         }
 
         /* image rejection calibration */
@@ -391,21 +426,21 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_MD2] = 0x08;
         regs[R_MD3] = 0x00;
 
-        tda18271_write_regs(fe, R_EP3, 11);
+        __tda18271_write_regs(fe, R_EP3, 11, false);
 
         if ((priv->id) == TDA18271HDC2) {
                 /* main pll cp source on */
-                tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 1);
+                __tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 1, false);
                 msleep(1);
 
                 /* main pll cp source off */
-                tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 0);
+                __tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 0, false);
         }
 
         msleep(5); /* pll locking */
 
         /* launch detector */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
         msleep(5); /* wanted low measurement */
 
         regs[R_EP5] = 0x85;
@@ -413,11 +448,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_CD1] = 0x66;
         regs[R_CD2] = 0x70;
 
-        tda18271_write_regs(fe, R_EP3, 7);
+        __tda18271_write_regs(fe, R_EP3, 7, false);
         msleep(5); /* pll locking */
 
         /* launch optimization algorithm */
-        tda18271_write_regs(fe, R_EP2, 1);
+        __tda18271_write_regs(fe, R_EP2, 1, false);
         msleep(30); /* image low optimization completion */
 
         /* mid-band */
@@ -428,11 +463,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_MD1] = 0x73;
         regs[R_MD2] = 0x1a;
 
-        tda18271_write_regs(fe, R_EP3, 11);
+        __tda18271_write_regs(fe, R_EP3, 11, false);
         msleep(5); /* pll locking */
 
         /* launch detector */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
         msleep(5); /* wanted mid measurement */
 
         regs[R_EP5] = 0x86;
@@ -440,11 +475,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_CD1] = 0x66;
         regs[R_CD2] = 0xa0;
 
-        tda18271_write_regs(fe, R_EP3, 7);
+        __tda18271_write_regs(fe, R_EP3, 7, false);
         msleep(5); /* pll locking */
 
         /* launch optimization algorithm */
-        tda18271_write_regs(fe, R_EP2, 1);
+        __tda18271_write_regs(fe, R_EP2, 1, false);
         msleep(30); /* image mid optimization completion */
 
         /* high-band */
@@ -456,30 +491,33 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_MD1] = 0x71;
         regs[R_MD2] = 0xcd;
 
-        tda18271_write_regs(fe, R_EP3, 11);
+        __tda18271_write_regs(fe, R_EP3, 11, false);
         msleep(5); /* pll locking */
 
         /* launch detector */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
         msleep(5); /* wanted high measurement */
 
         regs[R_EP5] = 0x87;
         regs[R_CD1] = 0x65;
         regs[R_CD2] = 0x50;
 
-        tda18271_write_regs(fe, R_EP3, 7);
+        __tda18271_write_regs(fe, R_EP3, 7, false);
         msleep(5); /* pll locking */
 
         /* launch optimization algorithm */
-        tda18271_write_regs(fe, R_EP2, 1);
+        __tda18271_write_regs(fe, R_EP2, 1, false);
         msleep(30); /* image high optimization completion */
 
         /* return to normal mode */
         regs[R_EP4] = 0x64;
-        tda18271_write_regs(fe, R_EP4, 1);
+        __tda18271_write_regs(fe, R_EP4, 1, false);
 
         /* synchronize */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
+
+        i2c_unlock_adapter(priv->i2c_props.adap);
+        tda18271_i2c_gate_ctrl(fe, 0);
 
         return 0;
 }
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 824f1911ee21..3d7526e28d42 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -500,7 +500,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
                 case 3:
                         state->af9013_config[i].clock = 25000000;
                         break;
-                };
+                }
                 dev_dbg(&d->udev->dev, "%s: [%d] xtal=%d set clock=%d\n",
                                 __func__, i, val,
                                 state->af9013_config[i].clock);
@@ -568,7 +568,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
                                         "supported, please report!\n",
                                         KBUILD_MODNAME, val);
                         return -ENODEV;
-                };
+                }
 
                 state->af9013_config[i].tuner = val;
                 dev_dbg(&d->udev->dev, "%s: [%d] tuner id=%d\n",
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index aabd3fc03ea7..ea27eaff4e34 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -520,7 +520,7 @@ static int af9035_read_config(struct dvb_usb_device *d)
                         dev_warn(&d->udev->dev, "%s: tuner id=%02x not " \
                                         "supported, please report!",
                                         KBUILD_MODNAME, tmp);
-                };
+                }
 
                 /* tuner IF frequency */
                 ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
diff --git a/drivers/media/usb/dvb-usb/a800.c b/drivers/media/usb/dvb-usb/a800.c
index 8d7fef84afd8..83684ed023cd 100644
--- a/drivers/media/usb/dvb-usb/a800.c
+++ b/drivers/media/usb/dvb-usb/a800.c
@@ -93,7 +93,7 @@ static int a800_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         /* call the universal NEC remote processor, to find out the key's state and event */
         dvb_usb_nec_rc_key_to_event(d,key,event,state);
         if (key[0] != 0)
-                deb_rc("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_rc("key: %*ph\n", 5, key);
         ret = 0;
 out:
         kfree(key);
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-core.c b/drivers/media/usb/dvb-usb/cinergyT2-core.c
index 0a98548ecd17..9fd1527494eb 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-core.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-core.c
@@ -172,8 +172,7 @@ static int cinergyt2_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
                 if (*event != d->last_event)
                         st->rc_counter = 0;
 
-                deb_rc("key: %x %x %x %x %x\n",
-                       key[0], key[1], key[2], key[3], key[4]);
+                deb_rc("key: %*ph\n", 5, key);
         }
         return 0;
 }
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index a76bbb29ca36..af0d4321845b 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -473,7 +473,7 @@ int dibusb_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         dvb_usb_generic_rw(d,&cmd,1,key,5,0);
         dvb_usb_nec_rc_key_to_event(d,key,event,state);
         if (key[0] != 0)
-                deb_info("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_info("key: %*ph\n", 5, key);
         return 0;
 }
 EXPORT_SYMBOL(dibusb_rc_query);
diff --git a/drivers/media/usb/dvb-usb/digitv.c b/drivers/media/usb/dvb-usb/digitv.c
index ff34419a4c88..772bde3c5020 100644
--- a/drivers/media/usb/dvb-usb/digitv.c
+++ b/drivers/media/usb/dvb-usb/digitv.c
@@ -253,7 +253,7 @@ static int digitv_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         }
 
         if (key[0] != 0)
-                deb_rc("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_rc("key: %*ph\n", 5, key);
         return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/dtt200u.c b/drivers/media/usb/dvb-usb/dtt200u.c
index 66f205c112b2..c357fb3b0a88 100644
--- a/drivers/media/usb/dvb-usb/dtt200u.c
+++ b/drivers/media/usb/dvb-usb/dtt200u.c
@@ -84,7 +84,7 @@ static int dtt200u_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         dvb_usb_generic_rw(d,&cmd,1,key,5,0);
         dvb_usb_nec_rc_key_to_event(d,key,event,state);
         if (key[0] != 0)
-                deb_info("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_info("key: %*ph\n", 5, key);
         return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/m920x.c b/drivers/media/usb/dvb-usb/m920x.c
index 288af29a8bb7..661bb75be955 100644
--- a/drivers/media/usb/dvb-usb/m920x.c
+++ b/drivers/media/usb/dvb-usb/m920x.c
@@ -358,7 +358,7 @@ static int m920x_firmware_download(struct usb_device *udev, const struct firmwar
 
         if ((ret = m920x_read(udev, M9206_FILTER, 0x0, 0x8000, read, 4)) != 0)
                 goto done;
-        deb("%x %x %x %x\n", read[0], read[1], read[2], read[3]);
+        deb("%*ph\n", 4, read);
 
         if ((ret = m920x_read(udev, M9206_FW, 0x0, 0x0, read, 1)) != 0)
                 goto done;
diff --git a/drivers/media/usb/dvb-usb/technisat-usb2.c b/drivers/media/usb/dvb-usb/technisat-usb2.c
index acefaa89cc53..7a8c8c18590f 100644
--- a/drivers/media/usb/dvb-usb/technisat-usb2.c
+++ b/drivers/media/usb/dvb-usb/technisat-usb2.c
@@ -677,6 +677,7 @@ static struct usb_device_id technisat_usb2_id_table[] = {
         { USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_DVB_S2) },
         { 0 }           /* Terminating entry */
 };
+MODULE_DEVICE_TABLE(usb, technisat_usb2_id_table);
 
 /* device description */
 static struct dvb_usb_device_properties technisat_usb2_devices = {
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index f7297ae76b48..16a84f9f46d8 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -2203,7 +2203,7 @@ EXPORT_SYMBOL_GPL(em28xx_tuner_callback);
 
 static inline void em28xx_set_model(struct em28xx *dev)
 {
-        memcpy(&dev->board, &em28xx_boards[dev->model], sizeof(dev->board));
+        dev->board = em28xx_boards[dev->model];
 
         /* Those are the default values for the majority of boards
            Use those values if not specified otherwise at boards entry
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index 913e5227897a..13ae821949e9 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -574,18 +574,19 @@ static void pctv_520e_init(struct em28xx *dev)
                 i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
 };
 
-static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe, int val)
+static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe)
 {
+        struct dtv_frontend_properties *c = &fe->dtv_property_cache;
         struct em28xx *dev = fe->dvb->priv;
 #ifdef CONFIG_GPIOLIB
         struct em28xx_dvb *dvb = dev->dvb;
         int ret;
         unsigned long flags;
 
-        if (val)
-                flags = GPIOF_OUT_INIT_LOW;
+        if (c->lna == 1)
+                flags = GPIOF_OUT_INIT_HIGH; /* enable LNA */
         else
-                flags = GPIOF_OUT_INIT_HIGH;
+                flags = GPIOF_OUT_INIT_LOW; /* disable LNA */
 
         ret = gpio_request_one(dvb->lna_gpio, flags, NULL);
         if (ret)
@@ -595,8 +596,8 @@ static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe, int val)
 
         return ret;
 #else
-        dev_warn(&dev->udev->dev, "%s: LNA control is disabled\n",
-                        KBUILD_MODNAME);
+        dev_warn(&dev->udev->dev, "%s: LNA control is disabled (lna=%u)\n",
+                        KBUILD_MODNAME, c->lna);
         return 0;
 #endif
 }
diff --git a/drivers/media/usb/stk1160/stk1160-core.c b/drivers/media/usb/stk1160/stk1160-core.c
index b62740846061..34a26e0cfe77 100644
--- a/drivers/media/usb/stk1160/stk1160-core.c
+++ b/drivers/media/usb/stk1160/stk1160-core.c
@@ -100,12 +100,21 @@ int stk1160_write_reg(struct stk1160 *dev, u16 reg, u16 value)
 
 void stk1160_select_input(struct stk1160 *dev)
 {
+        int route;
         static const u8 gctrl[] = {
-                0x98, 0x90, 0x88, 0x80
+                0x98, 0x90, 0x88, 0x80, 0x98
         };
 
-        if (dev->ctl_input < ARRAY_SIZE(gctrl))
+        if (dev->ctl_input == STK1160_SVIDEO_INPUT)
+                route = SAA7115_SVIDEO3;
+        else
+                route = SAA7115_COMPOSITE0;
+
+        if (dev->ctl_input < ARRAY_SIZE(gctrl)) {
+                v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
+                                route, 0, 0);
                 stk1160_write_reg(dev, STK1160_GCTRL, gctrl[dev->ctl_input]);
+        }
 }
 
 /* TODO: We should break this into pieces */
@@ -351,8 +360,6 @@ static int stk1160_probe(struct usb_interface *interface,
 
         /* i2c reset saa711x */
         v4l2_device_call_all(&dev->v4l2_dev, 0, core, reset, 0);
-        v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
-                                0, 0, 0);
         v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
 
         /* reset stk1160 to default values */
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index fe6e857969ca..6694f9e2ca57 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -419,7 +419,12 @@ static int vidioc_enum_input(struct file *file, void *priv,
         if (i->index > STK1160_MAX_INPUT)
                 return -EINVAL;
 
-        sprintf(i->name, "Composite%d", i->index);
+        /* S-Video special handling */
+        if (i->index == STK1160_SVIDEO_INPUT)
+                sprintf(i->name, "S-Video");
+        else
+                sprintf(i->name, "Composite%d", i->index);
+
         i->type = V4L2_INPUT_TYPE_CAMERA;
         i->std = dev->vdev.tvnorms;
         return 0;
diff --git a/drivers/media/usb/stk1160/stk1160.h b/drivers/media/usb/stk1160/stk1160.h
index 3feba0033f98..68c8707d36ab 100644
--- a/drivers/media/usb/stk1160/stk1160.h
+++ b/drivers/media/usb/stk1160/stk1160.h
@@ -46,7 +46,8 @@
 
 #define STK1160_MIN_PKT_SIZE 3072
 
-#define STK1160_MAX_INPUT 3
+#define STK1160_MAX_INPUT 4
+#define STK1160_SVIDEO_INPUT 4
 
 #define STK1160_I2C_TIMEOUT 100
 
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 5577381b5bf0..18a91fae6bc1 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -122,21 +122,27 @@ static struct vb2_ops uvc_queue_qops = {
         .buf_finish = uvc_buffer_finish,
 };
 
-void uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
+int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
                     int drop_corrupted)
 {
+        int ret;
+
         queue->queue.type = type;
         queue->queue.io_modes = VB2_MMAP | VB2_USERPTR;
         queue->queue.drv_priv = queue;
         queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
         queue->queue.ops = &uvc_queue_qops;
         queue->queue.mem_ops = &vb2_vmalloc_memops;
-        vb2_queue_init(&queue->queue);
+        ret = vb2_queue_init(&queue->queue);
+        if (ret)
+                return ret;
 
         mutex_init(&queue->mutex);
         spin_lock_init(&queue->irqlock);
         INIT_LIST_HEAD(&queue->irqqueue);
         queue->flags = drop_corrupted ? UVC_QUEUE_DROP_CORRUPTED : 0;
+
+        return 0;
 }
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 1c15b4227bdb..57c3076a4625 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -1755,7 +1755,9 @@ int uvc_video_init(struct uvc_streaming *stream)
         atomic_set(&stream->active, 0);
 
         /* Initialize the video buffers queue. */
-        uvc_queue_init(&stream->queue, stream->type, !uvc_no_drop_param);
+        ret = uvc_queue_init(&stream->queue, stream->type, !uvc_no_drop_param);
+        if (ret)
+                return ret;
 
         /* Alternate setting 0 should be the default, yet the XBox Live Vision
          * Cam (and possibly other devices) crash or otherwise misbehave if
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index 3764040475bb..af216ec45e39 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -600,7 +600,7 @@ extern struct uvc_driver uvc_driver;
 extern struct uvc_entity *uvc_entity_by_id(struct uvc_device *dev, int id);
 
 /* Video buffers queue management. */
-extern void uvc_queue_init(struct uvc_video_queue *queue,
+extern int uvc_queue_init(struct uvc_video_queue *queue,
                 enum v4l2_buf_type type, int drop_corrupted);
 extern int uvc_alloc_buffers(struct uvc_video_queue *queue,
                 struct v4l2_requestbuffers *rb);
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index 631cdc0e0bda..f6ee201d9347 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -384,6 +384,25 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 "Extended SAR",
                 NULL,
         };
+        static const char * const h264_fp_arrangement_type[] = {
+                "Checkerboard",
+                "Column",
+                "Row",
+                "Side by Side",
+                "Top Bottom",
+                "Temporal",
+                NULL,
+        };
+        static const char * const h264_fmo_map_type[] = {
+                "Interleaved Slices",
+                "Scattered Slices",
+                "Foreground with Leftover",
+                "Box Out",
+                "Raster Scan",
+                "Wipe Scan",
+                "Explicit",
+                NULL,
+        };
         static const char * const mpeg_mpeg4_level[] = {
                 "0",
                 "0b",
@@ -508,6 +527,10 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 return h264_profile;
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
                 return vui_sar_idc;
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+                return h264_fp_arrangement_type;
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:
+                return h264_fmo_map_type;
         case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
                 return mpeg_mpeg4_level;
         case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
@@ -643,6 +666,22 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH:        return "Horizontal Size of SAR";
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:           return "Aspect Ratio VUI Enable";
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:              return "VUI Aspect Ratio IDC";
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING:        return "H264 Enable Frame Packing SEI";
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_CURRENT_FRAME_0:   return "H264 Set Curr. Frame as Frame0";
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:  return "H264 FP Arrangement Type";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO:                      return "H264 Flexible MB Ordering";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:             return "H264 Map Type for FMO";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_SLICE_GROUP:          return "H264 FMO Number of Slice Groups";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_DIRECTION:     return "H264 FMO Direction of Change";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_RATE:          return "H264 FMO Size of 1st Slice Grp";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_RUN_LENGTH:           return "H264 FMO No. of Consecutive MBs";
+        case V4L2_CID_MPEG_VIDEO_H264_ASO:                      return "H264 Arbitrary Slice Ordering";
+        case V4L2_CID_MPEG_VIDEO_H264_ASO_SLICE_ORDER:          return "H264 ASO Slice Order";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING:      return "Enable H264 Hierarchical Coding";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE: return "H264 Hierarchical Coding Type";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER:return "H264 Number of HC Layers";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER_QP:
+                                                                return "H264 Set QP Value for HC Layers";
         case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:              return "MPEG4 I-Frame QP Value";
         case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:              return "MPEG4 P-Frame QP Value";
         case V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP:              return "MPEG4 B-Frame QP Value";
@@ -657,6 +696,7 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_MPEG_VIDEO_VBV_SIZE:                      return "VBV Buffer Size";
         case V4L2_CID_MPEG_VIDEO_DEC_PTS:                       return "Video Decoder PTS";
         case V4L2_CID_MPEG_VIDEO_DEC_FRAME:                     return "Video Decoder Frame Count";
+        case V4L2_CID_MPEG_VIDEO_VBV_DELAY:                     return "Initial Delay for VBV Control";
 
         /* CAMERA controls */
         /* Keep the order of the 'case's the same as in videodev2.h! */
@@ -749,6 +789,7 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_IMAGE_PROC_CLASS:         return "Image Processing Controls";
         case V4L2_CID_LINK_FREQ:                return "Link Frequency";
         case V4L2_CID_PIXEL_RATE:               return "Pixel Rate";
+        case V4L2_CID_TEST_PATTERN:             return "Test Pattern";
 
         /* DV controls */
         case V4L2_CID_DV_CLASS:                 return "Digital Video Controls";
@@ -853,6 +894,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:
         case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
         case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
         case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
@@ -862,6 +905,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_DV_TX_MODE:
         case V4L2_CID_DV_TX_RGB_RANGE:
         case V4L2_CID_DV_RX_RGB_RANGE:
+        case V4L2_CID_TEST_PATTERN:
                 *type = V4L2_CTRL_TYPE_MENU;
                 break;
         case V4L2_CID_LINK_FREQ:
@@ -1648,6 +1692,36 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu);
 
+/* Helper function for standard menu controls with driver defined menu */
+struct v4l2_ctrl *v4l2_ctrl_new_std_menu_items(struct v4l2_ctrl_handler *hdl,
+                        const struct v4l2_ctrl_ops *ops, u32 id, s32 max,
+                        s32 mask, s32 def, const char * const *qmenu)
+{
+        enum v4l2_ctrl_type type;
+        const char *name;
+        u32 flags;
+        s32 step;
+        s32 min;
+
+        /* v4l2_ctrl_new_std_menu_items() should only be called for
+         * standard controls without a standard menu.
+         */
+        if (v4l2_ctrl_get_menu(id)) {
+                handler_set_err(hdl, -EINVAL);
+                return NULL;
+        }
+
+        v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
+        if (type != V4L2_CTRL_TYPE_MENU || qmenu == NULL) {
+                handler_set_err(hdl, -EINVAL);
+                return NULL;
+        }
+        return v4l2_ctrl_new(hdl, ops, id, name, type, 0, max, mask, def,
+                             flags, qmenu, NULL, NULL);
+
+}
+EXPORT_SYMBOL(v4l2_ctrl_new_std_menu_items);
+
 /* Helper function for standard integer menu controls */
 struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
                         const struct v4l2_ctrl_ops *ops,
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 9d3e46c446ad..8f388ff31ebb 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -157,8 +157,7 @@ static const char *v4l2_memory_names[] = {
         [V4L2_MEMORY_OVERLAY] = "overlay",
 };
 
-#define prt_names(a, arr) ((((a) >= 0) && ((a) < ARRAY_SIZE(arr))) ? \
-                           arr[a] : "unknown")
+#define prt_names(a, arr) (((unsigned)(a)) < ARRAY_SIZE(arr) ? arr[a] : "unknown")
 
 /* ------------------------------------------------------------------ */
 /* debug help functions                                               */
@@ -2188,6 +2187,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
         int ret = 0;
 
         switch (cmd) {
+        case VIDIOC_PREPARE_BUF:
         case VIDIOC_QUERYBUF:
         case VIDIOC_QBUF:
         case VIDIOC_DQBUF: {
@@ -2211,6 +2211,10 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
                 struct v4l2_subdev_edid *edid = parg;
 
                 if (edid->blocks) {
+                        if (edid->blocks > 256) {
+                                ret = -EINVAL;
+                                break;
+                        }
                         *user_ptr = (void __user *)edid->edid;
                         *kernel_ptr = (void *)&edid->edid;
                         *array_size = edid->blocks * 128;
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index e6a26b433e87..432df119af27 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -276,6 +276,9 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
  */
 static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
+        if (!V4L2_TYPE_IS_MULTIPLANAR(b->type))
+                return 0;
+
         /* Is memory for copying plane information present? */
         if (NULL == b->m.planes) {
                 dprintk(1, "Multi-planar buffer passed but "
@@ -331,10 +334,9 @@ static bool __buffers_in_use(struct vb2_queue *q)
  * __fill_v4l2_buffer() - fill in a struct v4l2_buffer with information to be
  * returned to userspace
  */
-static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
+static void __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 {
         struct vb2_queue *q = vb->vb2_queue;
-        int ret;
 
         /* Copy back data such as timestamp, flags, etc. */
         memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
@@ -342,14 +344,11 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
         b->reserved = vb->v4l2_buf.reserved;
 
         if (V4L2_TYPE_IS_MULTIPLANAR(q->type)) {
-                ret = __verify_planes_array(vb, b);
-                if (ret)
-                        return ret;
-
                 /*
                  * Fill in plane-related data if userspace provided an array
-                 * for it. The memory and size is verified above.
+                 * for it. The caller has already verified memory and size.
                  */
+                b->length = vb->num_planes;
                 memcpy(b->m.planes, vb->v4l2_planes,
                         b->length * sizeof(struct v4l2_plane));
         } else {
@@ -391,8 +390,6 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 
         if (__buffer_in_use(q, vb))
                 b->flags |= V4L2_BUF_FLAG_MAPPED;
-
-        return 0;
 }
 
 /**
@@ -411,6 +408,7 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
         struct vb2_buffer *vb;
+        int ret;
 
         if (b->type != q->type) {
                 dprintk(1, "querybuf: wrong buffer type\n");
@@ -422,8 +420,10 @@ int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
                 return -EINVAL;
         }
         vb = q->bufs[b->index];
-
-        return __fill_v4l2_buffer(vb, b);
+        ret = __verify_planes_array(vb, b);
+        if (!ret)
+                __fill_v4l2_buffer(vb, b);
+        return ret;
 }
 EXPORT_SYMBOL(vb2_querybuf);
 
@@ -813,24 +813,16 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 EXPORT_SYMBOL_GPL(vb2_buffer_done);
 
 /**
- * __fill_vb2_buffer() - fill a vb2_buffer with information provided in
- * a v4l2_buffer by the userspace
+ * __fill_vb2_buffer() - fill a vb2_buffer with information provided in a
+ * v4l2_buffer by the userspace. The caller has already verified that struct
+ * v4l2_buffer has a valid number of planes.
  */
-static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
+static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
                                 struct v4l2_plane *v4l2_planes)
 {
         unsigned int plane;
-        int ret;
 
         if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
-                /*
-                 * Verify that the userspace gave us a valid array for
-                 * plane information.
-                 */
-                ret = __verify_planes_array(vb, b);
-                if (ret)
-                        return ret;
-
                 /* Fill in driver-provided information for OUTPUT types */
                 if (V4L2_TYPE_IS_OUTPUT(b->type)) {
                         /*
@@ -872,8 +864,6 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
         vb->v4l2_buf.field = b->field;
         vb->v4l2_buf.timestamp = b->timestamp;
         vb->v4l2_buf.flags = b->flags & ~V4L2_BUFFER_STATE_FLAGS;
-
-        return 0;
 }
 
 /**
@@ -888,10 +878,8 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
         int ret;
         int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 
-        /* Verify and copy relevant information provided by the userspace */
-        ret = __fill_vb2_buffer(vb, b, planes);
-        if (ret)
-                return ret;
+        /* Copy relevant information provided by the userspace */
+        __fill_vb2_buffer(vb, b, planes);
 
         for (plane = 0; plane < vb->num_planes; ++plane) {
                 /* Skip the plane if already verified */
@@ -966,7 +954,8 @@ err:
  */
 static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
-        return __fill_vb2_buffer(vb, b, vb->v4l2_planes);
+        __fill_vb2_buffer(vb, b, vb->v4l2_planes);
+        return 0;
 }
 
 /**
@@ -1059,7 +1048,9 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
                 dprintk(1, "%s(): invalid buffer state %d\n", __func__, vb->state);
                 return -EINVAL;
         }
-
+        ret = __verify_planes_array(vb, b);
+        if (ret < 0)
+                return ret;
         ret = __buf_prepare(vb, b);
         if (ret < 0)
                 return ret;
@@ -1147,6 +1138,9 @@ int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
                 ret = -EINVAL;
                 goto unlock;
         }
+        ret = __verify_planes_array(vb, b);
+        if (ret)
+                goto unlock;
 
         switch (vb->state) {
         case VB2_BUF_STATE_DEQUEUED:
@@ -1243,8 +1237,10 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
                  * the locks or return an error if one occurred.
                  */
                 call_qop(q, wait_finish, q);
-                if (ret)
+                if (ret) {
+                        dprintk(1, "Sleep was interrupted\n");
                         return ret;
+                }
         }
         return 0;
 }
@@ -1255,7 +1251,7 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
  * Will sleep if required for nonblocking == false.
  */
 static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
-                                int nonblocking)
+                                struct v4l2_buffer *b, int nonblocking)
 {
         unsigned long flags;
         int ret;
@@ -1273,10 +1269,16 @@ static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
          */
         spin_lock_irqsave(&q->done_lock, flags);
         *vb = list_first_entry(&q->done_list, struct vb2_buffer, done_entry);
-        list_del(&(*vb)->done_entry);
+        /*
+         * Only remove the buffer from done_list if v4l2_buffer can handle all
+         * the planes.
+         */
+        ret = __verify_planes_array(*vb, b);
+        if (!ret)
+                list_del(&(*vb)->done_entry);
         spin_unlock_irqrestore(&q->done_lock, flags);
 
-        return 0;
+        return ret;
 }
 
 /**
@@ -1335,12 +1337,9 @@ int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
                 dprintk(1, "dqbuf: invalid buffer type\n");
                 return -EINVAL;
         }
-
-        ret = __vb2_get_done_vb(q, &vb, nonblocking);
-        if (ret < 0) {
-                dprintk(1, "dqbuf: error getting next done buffer\n");
+        ret = __vb2_get_done_vb(q, &vb, b, nonblocking);
+        if (ret < 0)
                 return ret;
-        }
 
         ret = call_qop(q, buf_finish, vb);
         if (ret) {
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 99c73352c430..b151b7c1bd59 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -60,16 +60,6 @@ config ATMEL_PWM
           purposes including software controlled power-efficient backlights
           on LCD displays, motor control, and waveform generation.
 
-config AB8500_PWM
-        bool "AB8500 PWM support"
-        depends on AB8500_CORE && ARCH_U8500
-        select HAVE_PWM
-        depends on !PWM
-        help
-          This driver exports functions to enable/disble/config/free Pulse
-          Width Modulation in the Analog Baseband Chip AB8500.
-          It is used by led and backlight driver to control the intensity.
-
 config ATMEL_TCLIB
         bool "Atmel AT32/AT91 Timer/Counter Library"
         depends on (AVR32 || ARCH_AT91)
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index b88df7a350b8..2129377c0de6 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -44,7 +44,6 @@ obj-$(CONFIG_VMWARE_BALLOON)	+= vmw_balloon.o
 obj-$(CONFIG_ARM_CHARLCD)       += arm-charlcd.o
 obj-$(CONFIG_PCH_PHUB)          += pch_phub.o
 obj-y                           += ti-st/
-obj-$(CONFIG_AB8500_PWM)        += ab8500-pwm.o
 obj-y                           += lis3lv02d/
 obj-y                           += carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 8ac5246e2ab2..06c42cfb7c34 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -26,6 +26,7 @@
 #include <linux/suspend.h>
 #include <linux/fault-inject.h>
 #include <linux/random.h>
+#include <linux/slab.h>
 
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -41,6 +42,12 @@
 #include "sd_ops.h"
 #include "sdio_ops.h"
 
+/*
+ * Background operations can take a long time, depending on the housekeeping
+ * operations the card has to perform.
+ */
+#define MMC_BKOPS_MAX_TIMEOUT   (4 * 60 * 1000) /* max time to wait in ms */
+
 static struct workqueue_struct *workqueue;
 static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 
@@ -245,6 +252,70 @@ mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
         host->ops->request(host, mrq);
 }
 
+/**
+ *      mmc_start_bkops - start BKOPS for supported cards
+ *      @card: MMC card to start BKOPS
+ *      @form_exception: A flag to indicate if this function was
+ *                       called due to an exception raised by the card
+ *
+ *      Start background operations whenever requested.
+ *      When the urgent BKOPS bit is set in a R1 command response
+ *      then background operations should be started immediately.
+*/
+void mmc_start_bkops(struct mmc_card *card, bool from_exception)
+{
+        int err;
+        int timeout;
+        bool use_busy_signal;
+
+        BUG_ON(!card);
+
+        if (!card->ext_csd.bkops_en || mmc_card_doing_bkops(card))
+                return;
+
+        err = mmc_read_bkops_status(card);
+        if (err) {
+                pr_err("%s: Failed to read bkops status: %d\n",
+                       mmc_hostname(card->host), err);
+                return;
+        }
+
+        if (!card->ext_csd.raw_bkops_status)
+                return;
+
+        if (card->ext_csd.raw_bkops_status < EXT_CSD_BKOPS_LEVEL_2 &&
+            from_exception)
+                return;
+
+        mmc_claim_host(card->host);
+        if (card->ext_csd.raw_bkops_status >= EXT_CSD_BKOPS_LEVEL_2) {
+                timeout = MMC_BKOPS_MAX_TIMEOUT;
+                use_busy_signal = true;
+        } else {
+                timeout = 0;
+                use_busy_signal = false;
+        }
+
+        err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+                        EXT_CSD_BKOPS_START, 1, timeout, use_busy_signal);
+        if (err) {
+                pr_warn("%s: Error %d starting bkops\n",
+                        mmc_hostname(card->host), err);
+                goto out;
+        }
+
+        /*
+         * For urgent bkops status (LEVEL_2 and more)
+         * bkops executed synchronously, otherwise
+         * the operation is in progress
+         */
+        if (!use_busy_signal)
+                mmc_card_set_doing_bkops(card);
+out:
+        mmc_release_host(card->host);
+}
+EXPORT_SYMBOL(mmc_start_bkops);
+
 static void mmc_wait_done(struct mmc_request *mrq)
 {
         complete(&mrq->completion);
@@ -354,6 +425,14 @@ struct mmc_async_req *mmc_start_req(struct mmc_host *host,
         if (host->areq) {
                 mmc_wait_for_req_done(host, host->areq->mrq);
                 err = host->areq->err_check(host->card, host->areq);
+                /*
+                 * Check BKOPS urgency for each R1 response
+                 */
+                if (host->card && mmc_card_mmc(host->card) &&
+                    ((mmc_resp_type(host->areq->mrq->cmd) == MMC_RSP_R1) ||
+                     (mmc_resp_type(host->areq->mrq->cmd) == MMC_RSP_R1B)) &&
+                    (host->areq->mrq->cmd->resp[0] & R1_EXCEPTION_EVENT))
+                        mmc_start_bkops(host->card, true);
         }
 
         if (!err && areq)
@@ -398,7 +477,7 @@ EXPORT_SYMBOL(mmc_wait_for_req);
  *      @card: the MMC card associated with the HPI transfer
  *
  *      Issued High Priority Interrupt, and check for card status
- *      util out-of prg-state.
+ *      until out-of prg-state.
  */
 int mmc_interrupt_hpi(struct mmc_card *card)
 {
@@ -424,8 +503,9 @@ int mmc_interrupt_hpi(struct mmc_card *card)
         case R1_STATE_IDLE:
         case R1_STATE_READY:
         case R1_STATE_STBY:
+        case R1_STATE_TRAN:
                 /*
-                 * In idle states, HPI is not needed and the caller
+                 * In idle and transfer states, HPI is not needed and the caller
                  * can issue the next intended command immediately
                  */
                 goto out;
@@ -489,6 +569,64 @@ int mmc_wait_for_cmd(struct mmc_host *host, struct mmc_command *cmd, int retries
 EXPORT_SYMBOL(mmc_wait_for_cmd);
 
 /**
+ *      mmc_stop_bkops - stop ongoing BKOPS
+ *      @card: MMC card to check BKOPS
+ *
+ *      Send HPI command to stop ongoing background operations to
+ *      allow rapid servicing of foreground operations, e.g. read/
+ *      writes. Wait until the card comes out of the programming state
+ *      to avoid errors in servicing read/write requests.
+ */
+int mmc_stop_bkops(struct mmc_card *card)
+{
+        int err = 0;
+
+        BUG_ON(!card);
+        err = mmc_interrupt_hpi(card);
+
+        /*
+         * If err is EINVAL, we can't issue an HPI.
+         * It should complete the BKOPS.
+         */
+        if (!err || (err == -EINVAL)) {
+                mmc_card_clr_doing_bkops(card);
+                err = 0;
+        }
+
+        return err;
+}
+EXPORT_SYMBOL(mmc_stop_bkops);
+
+int mmc_read_bkops_status(struct mmc_card *card)
+{
+        int err;
+        u8 *ext_csd;
+
+        /*
+         * In future work, we should consider storing the entire ext_csd.
+         */
+        ext_csd = kmalloc(512, GFP_KERNEL);
+        if (!ext_csd) {
+                pr_err("%s: could not allocate buffer to receive the ext_csd.\n",
+                       mmc_hostname(card->host));
+                return -ENOMEM;
+        }
+
+        mmc_claim_host(card->host);
+        err = mmc_send_ext_csd(card, ext_csd);
+        mmc_release_host(card->host);
+        if (err)
+                goto out;
+
+        card->ext_csd.raw_bkops_status = ext_csd[EXT_CSD_BKOPS_STATUS];
+        card->ext_csd.raw_exception_status = ext_csd[EXT_CSD_EXP_EVENTS_STATUS];
+out:
+        kfree(ext_csd);
+        return err;
+}
+EXPORT_SYMBOL(mmc_read_bkops_status);
+
+/**
  *      mmc_set_data_timeout - set the timeout for a data command
  *      @data: data phase for command
  *      @card: the MMC card associated with the data transfer
@@ -975,7 +1113,8 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
                 int             tmp;
                 int             voltage;
 
-                /* REVISIT mmc_vddrange_to_ocrmask() may have set some
+                /*
+                 * REVISIT mmc_vddrange_to_ocrmask() may have set some
                  * bits this regulator doesn't quite support ... don't
                  * be too picky, most cards and regulators are OK with
                  * a 0.1V range goof (it's a small error percentage).
@@ -989,12 +1128,13 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
                         max_uV = min_uV + 100 * 1000;
                 }
 
-                /* avoid needless changes to this voltage; the regulator
-                 * might not allow this operation
+                /*
+                 * If we're using a fixed/static regulator, don't call
+                 * regulator_set_voltage; it would fail.
                  */
                 voltage = regulator_get_voltage(supply);
 
-                if (mmc->caps2 & MMC_CAP2_BROKEN_VOLTAGE)
+                if (regulator_count_voltages(supply) == 1)
                         min_uV = max_uV = voltage;
 
                 if (voltage < 0)
@@ -1133,48 +1273,6 @@ void mmc_set_driver_type(struct mmc_host *host, unsigned int drv_type)
         mmc_host_clk_release(host);
 }
 
-static void mmc_poweroff_notify(struct mmc_host *host)
-{
-        struct mmc_card *card;
-        unsigned int timeout;
-        unsigned int notify_type = EXT_CSD_NO_POWER_NOTIFICATION;
-        int err = 0;
-
-        card = host->card;
-        mmc_claim_host(host);
-
-        /*
-         * Send power notify command only if card
-         * is mmc and notify state is powered ON
-         */
-        if (card && mmc_card_mmc(card) &&
-            (card->poweroff_notify_state == MMC_POWERED_ON)) {
-
-                if (host->power_notify_type == MMC_HOST_PW_NOTIFY_SHORT) {
-                        notify_type = EXT_CSD_POWER_OFF_SHORT;
-                        timeout = card->ext_csd.generic_cmd6_time;
-                        card->poweroff_notify_state = MMC_POWEROFF_SHORT;
-                } else {
-                        notify_type = EXT_CSD_POWER_OFF_LONG;
-                        timeout = card->ext_csd.power_off_longtime;
-                        card->poweroff_notify_state = MMC_POWEROFF_LONG;
-                }
-
-                err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-                                 EXT_CSD_POWER_OFF_NOTIFICATION,
-                                 notify_type, timeout);
-
-                if (err && err != -EBADMSG)
-                        pr_err("Device failed to respond within %d poweroff "
-                               "time. Forcefully powering down the device\n",
-                               timeout);
-
-                /* Set the card state to no notification after the poweroff */
-                card->poweroff_notify_state = MMC_NO_POWER_NOTIFICATION;
-        }
-        mmc_release_host(host);
-}
-
 /*
  * Apply power to the MMC stack.  This is a two-stage process.
  * First, we enable power to the card without the clock running.
@@ -1237,8 +1335,6 @@ static void mmc_power_up(struct mmc_host *host)
 
 void mmc_power_off(struct mmc_host *host)
 {
-        int err = 0;
-
         if (host->ios.power_mode == MMC_POWER_OFF)
                 return;
 
@@ -1247,22 +1343,6 @@ void mmc_power_off(struct mmc_host *host)
         host->ios.clock = 0;
         host->ios.vdd = 0;
 
-        /*
-         * For eMMC 4.5 device send AWAKE command before
-         * POWER_OFF_NOTIFY command, because in sleep state
-         * eMMC 4.5 devices respond to only RESET and AWAKE cmd
-         */
-        if (host->card && mmc_card_is_sleep(host->card) &&
-            host->bus_ops->resume) {
-                err = host->bus_ops->resume(host);
-
-                if (!err)
-                        mmc_poweroff_notify(host);
-                else
-                        pr_warning("%s: error %d during resume "
-                                   "(continue with poweroff sequence)\n",
-                                   mmc_hostname(host), err);
-        }
 
         /*
          * Reset ocr mask to be the highest possible voltage supported for
@@ -2052,6 +2132,11 @@ void mmc_rescan(struct work_struct *work)
         if (host->rescan_disable)
                 return;
 
+        /* If there is a non-removable card registered, only scan once */
+        if ((host->caps & MMC_CAP_NONREMOVABLE) && host->rescan_entered)
+                return;
+        host->rescan_entered = 1;
+
         mmc_bus_get(host);
 
         /*
@@ -2327,9 +2412,14 @@ int mmc_suspend_host(struct mmc_host *host)
 
         mmc_bus_get(host);
         if (host->bus_ops && !host->bus_dead) {
-
-                if (host->bus_ops->suspend)
+                if (host->bus_ops->suspend) {
+                        if (mmc_card_doing_bkops(host->card)) {
+                                err = mmc_stop_bkops(host->card);
+                                if (err)
+                                        goto out;
+                        }
                         err = host->bus_ops->suspend(host);
+                }
 
                 if (err == -ENOSYS || !host->bus_ops->resume) {
                         /*
@@ -2411,15 +2501,24 @@ int mmc_pm_notify(struct notifier_block *notify_block,
         struct mmc_host *host = container_of(
                 notify_block, struct mmc_host, pm_notify);
         unsigned long flags;
-
+        int err = 0;
 
         switch (mode) {
         case PM_HIBERNATION_PREPARE:
         case PM_SUSPEND_PREPARE:
+                if (host->card && mmc_card_mmc(host->card) &&
+                    mmc_card_doing_bkops(host->card)) {
+                        err = mmc_stop_bkops(host->card);
+                        if (err) {
+                                pr_err("%s: didn't stop bkops\n",
+                                        mmc_hostname(host));
+                                return err;
+                        }
+                        mmc_card_clr_doing_bkops(host->card);
+                }
 
                 spin_lock_irqsave(&host->lock, flags);
                 host->rescan_disable = 1;
-                host->power_notify_type = MMC_HOST_PW_NOTIFY_SHORT;
                 spin_unlock_irqrestore(&host->lock, flags);
                 cancel_delayed_work_sync(&host->detect);
 
@@ -2443,7 +2542,6 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 
                 spin_lock_irqsave(&host->lock, flags);
                 host->rescan_disable = 0;
-                host->power_notify_type = MMC_HOST_PW_NOTIFY_LONG;
                 spin_unlock_irqrestore(&host->lock, flags);
                 mmc_detect_change(host, 0);
 
diff --git a/drivers/mmc/core/debugfs.c b/drivers/mmc/core/debugfs.c
index 9ab5b17d488a..d96c643dde1c 100644
--- a/drivers/mmc/core/debugfs.c
+++ b/drivers/mmc/core/debugfs.c
@@ -281,7 +281,7 @@ static int mmc_ext_csd_open(struct inode *inode, struct file *filp)
         if (err)
                 goto out_free;
 
-        for (i = 511; i >= 0; i--)
+        for (i = 0; i < 512; i++)
                 n += sprintf(buf + n, "%02x", ext_csd[i]);
         n += sprintf(buf + n, "\n");
         BUG_ON(n != EXT_CSD_STR_LEN);
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 396b25891bb9..7cc46382fd64 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -463,6 +463,17 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
         }
 
         if (card->ext_csd.rev >= 5) {
+                /* check whether the eMMC card supports BKOPS */
+                if (ext_csd[EXT_CSD_BKOPS_SUPPORT] & 0x1) {
+                        card->ext_csd.bkops = 1;
+                        card->ext_csd.bkops_en = ext_csd[EXT_CSD_BKOPS_EN];
+                        card->ext_csd.raw_bkops_status =
+                                ext_csd[EXT_CSD_BKOPS_STATUS];
+                        if (!card->ext_csd.bkops_en)
+                                pr_info("%s: BKOPS_EN bit is not set\n",
+                                        mmc_hostname(card->host));
+                }
+
                 /* check whether the eMMC card supports HPI */
                 if (ext_csd[EXT_CSD_HPI_FEATURES] & 0x1) {
                         card->ext_csd.hpi = 1;
@@ -996,7 +1007,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
                  * so check for success and update the flag
                  */
                 if (!err)
-                        card->poweroff_notify_state = MMC_POWERED_ON;
+                        card->ext_csd.power_off_notification = EXT_CSD_POWER_ON;
         }
 
         /*
@@ -1262,6 +1273,35 @@ err:
         return err;
 }
 
+static int mmc_can_poweroff_notify(const struct mmc_card *card)
+{
+        return card &&
+                mmc_card_mmc(card) &&
+                (card->ext_csd.power_off_notification == EXT_CSD_POWER_ON);
+}
+
+static int mmc_poweroff_notify(struct mmc_card *card, unsigned int notify_type)
+{
+        unsigned int timeout = card->ext_csd.generic_cmd6_time;
+        int err;
+
+        /* Use EXT_CSD_POWER_OFF_SHORT as default notification type. */
+        if (notify_type == EXT_CSD_POWER_OFF_LONG)
+                timeout = card->ext_csd.power_off_longtime;
+
+        err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+                         EXT_CSD_POWER_OFF_NOTIFICATION,
+                         notify_type, timeout);
+        if (err)
+                pr_err("%s: Power Off Notification timed out, %u\n",
+                       mmc_hostname(card->host), timeout);
+
+        /* Disable the power off notification after the switch operation. */
+        card->ext_csd.power_off_notification = EXT_CSD_NO_POWER_NOTIFICATION;
+
+        return err;
+}
+
 /*
  * Host is being removed. Free up the current card.
  */
@@ -1322,11 +1362,11 @@ static int mmc_suspend(struct mmc_host *host)
         BUG_ON(!host->card);
 
         mmc_claim_host(host);
-        if (mmc_card_can_sleep(host)) {
+        if (mmc_can_poweroff_notify(host->card))
+                err = mmc_poweroff_notify(host->card, EXT_CSD_POWER_OFF_SHORT);
+        else if (mmc_card_can_sleep(host))
                 err = mmc_card_sleep(host);
-                if (!err)
-                        mmc_card_set_sleep(host->card);
-        } else if (!mmc_host_is_spi(host))
+        else if (!mmc_host_is_spi(host))
                 err = mmc_deselect_cards(host);
         host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
         mmc_release_host(host);
@@ -1348,11 +1388,7 @@ static int mmc_resume(struct mmc_host *host)
         BUG_ON(!host->card);
 
         mmc_claim_host(host);
-        if (mmc_card_is_sleep(host->card)) {
-                err = mmc_card_awake(host);
-                mmc_card_clr_sleep(host->card);
-        } else
-                err = mmc_init_card(host, host->ocr, host->card);
+        err = mmc_init_card(host, host->ocr, host->card);
         mmc_release_host(host);
 
         return err;
@@ -1363,7 +1399,6 @@ static int mmc_power_restore(struct mmc_host *host)
         int ret;
 
         host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
-        mmc_card_clr_sleep(host->card);
         mmc_claim_host(host);
         ret = mmc_init_card(host, host->ocr, host->card);
         mmc_release_host(host);
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 0ed2cc5f35b6..a0e172042e65 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -230,6 +230,10 @@ mmc_send_cxd_native(struct mmc_host *host, u32 arg, u32 *cxd, int opcode)
         return 0;
 }
 
+/*
+ * NOTE: void *buf, caller for the buf is required to use DMA-capable
+ * buffer or on-stack buffer (with some overhead in callee).
+ */
 static int
 mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
                 u32 opcode, void *buf, unsigned len)
@@ -239,13 +243,19 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
         struct mmc_data data = {0};
         struct scatterlist sg;
         void *data_buf;
+        int is_on_stack;
 
-        /* dma onto stack is unsafe/nonportable, but callers to this
-         * routine normally provide temporary on-stack buffers ...
-         */
-        data_buf = kmalloc(len, GFP_KERNEL);
-        if (data_buf == NULL)
-                return -ENOMEM;
+        is_on_stack = object_is_on_stack(buf);
+        if (is_on_stack) {
+                /*
+                 * dma onto stack is unsafe/nonportable, but callers to this
+                 * routine normally provide temporary on-stack buffers ...
+                 */
+                data_buf = kmalloc(len, GFP_KERNEL);
+                if (!data_buf)
+                        return -ENOMEM;
+        } else
+                data_buf = buf;
 
         mrq.cmd = &cmd;
         mrq.data = &data;
@@ -280,8 +290,10 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 
         mmc_wait_for_req(host, &mrq);
 
-        memcpy(buf, data_buf, len);
-        kfree(data_buf);
+        if (is_on_stack) {
+                memcpy(buf, data_buf, len);
+                kfree(data_buf);
+        }
 
         if (cmd.error)
                 return cmd.error;
@@ -294,24 +306,32 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 int mmc_send_csd(struct mmc_card *card, u32 *csd)
 {
         int ret, i;
+        u32 *csd_tmp;
 
         if (!mmc_host_is_spi(card->host))
                 return mmc_send_cxd_native(card->host, card->rca << 16,
                                 csd, MMC_SEND_CSD);
 
-        ret = mmc_send_cxd_data(card, card->host, MMC_SEND_CSD, csd, 16);
+        csd_tmp = kmalloc(16, GFP_KERNEL);
+        if (!csd_tmp)
+                return -ENOMEM;
+
+        ret = mmc_send_cxd_data(card, card->host, MMC_SEND_CSD, csd_tmp, 16);
         if (ret)
-                return ret;
+                goto err;
 
         for (i = 0;i < 4;i++)
-                csd[i] = be32_to_cpu(csd[i]);
+                csd[i] = be32_to_cpu(csd_tmp[i]);
 
-        return 0;
+err:
+        kfree(csd_tmp);
+        return ret;
 }
 
 int mmc_send_cid(struct mmc_host *host, u32 *cid)
 {
         int ret, i;
+        u32 *cid_tmp;
 
         if (!mmc_host_is_spi(host)) {
                 if (!host->card)
@@ -320,14 +340,20 @@ int mmc_send_cid(struct mmc_host *host, u32 *cid)
                                 cid, MMC_SEND_CID);
         }
 
-        ret = mmc_send_cxd_data(NULL, host, MMC_SEND_CID, cid, 16);
+        cid_tmp = kmalloc(16, GFP_KERNEL);
+        if (!cid_tmp)
+                return -ENOMEM;
+
+        ret = mmc_send_cxd_data(NULL, host, MMC_SEND_CID, cid_tmp, 16);
         if (ret)
-                return ret;
+                goto err;
 
         for (i = 0;i < 4;i++)
-                cid[i] = be32_to_cpu(cid[i]);
+                cid[i] = be32_to_cpu(cid_tmp[i]);
 
-        return 0;
+err:
+        kfree(cid_tmp);
+        return ret;
 }
 
 int mmc_send_ext_csd(struct mmc_card *card, u8 *ext_csd)
@@ -367,18 +393,19 @@ int mmc_spi_set_crc(struct mmc_host *host, int use_crc)
 }
 
 /**
- *      mmc_switch - modify EXT_CSD register
+ *      __mmc_switch - modify EXT_CSD register
  *      @card: the MMC card associated with the data transfer
  *      @set: cmd set values
  *      @index: EXT_CSD register index
  *      @value: value to program into EXT_CSD register
  *      @timeout_ms: timeout (ms) for operation performed by register write,
  *                   timeout of zero implies maximum possible timeout
+ *      @use_busy_signal: use the busy signal as response type
  *
  *      Modifies the EXT_CSD register for selected card.
  */
-int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
-               unsigned int timeout_ms)
+int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
+               unsigned int timeout_ms, bool use_busy_signal)
 {
         int err;
         struct mmc_command cmd = {0};
@@ -392,13 +419,23 @@ int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
                   (index << 16) |
                   (value << 8) |
                   set;
-        cmd.flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
+        cmd.flags = MMC_CMD_AC;
+        if (use_busy_signal)
+                cmd.flags |= MMC_RSP_SPI_R1B | MMC_RSP_R1B;
+        else
+                cmd.flags |= MMC_RSP_SPI_R1 | MMC_RSP_R1;
+
+
         cmd.cmd_timeout_ms = timeout_ms;
 
         err = mmc_wait_for_cmd(card->host, &cmd, MMC_CMD_RETRIES);
         if (err)
                 return err;
 
+        /* No need to check card status in case of unblocking command */
+        if (!use_busy_signal)
+                return 0;
+
         /* Must check status to be sure of no errors */
         do {
                 err = mmc_send_status(card, &status);
@@ -423,6 +460,13 @@ int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 
         return 0;
 }
+EXPORT_SYMBOL_GPL(__mmc_switch);
+
+int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
+                unsigned int timeout_ms)
+{
+        return __mmc_switch(card, set, index, value, timeout_ms, true);
+}
 EXPORT_SYMBOL_GPL(mmc_switch);
 
 int mmc_send_status(struct mmc_card *card, u32 *status)
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index 236842ec955a..6bf68799fe97 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -193,14 +193,7 @@ static int sdio_bus_remove(struct device *dev)
 }
 
 #ifdef CONFIG_PM
-
-static int pm_no_operation(struct device *dev)
-{
-        return 0;
-}
-
 static const struct dev_pm_ops sdio_bus_pm_ops = {
-        SET_SYSTEM_SLEEP_PM_OPS(pm_no_operation, pm_no_operation)
         SET_RUNTIME_PM_OPS(
                 pm_generic_runtime_suspend,
                 pm_generic_runtime_resume,
diff --git a/drivers/mmc/core/slot-gpio.c b/drivers/mmc/core/slot-gpio.c
index 058242916cef..08c6b3dfe080 100644
--- a/drivers/mmc/core/slot-gpio.c
+++ b/drivers/mmc/core/slot-gpio.c
@@ -100,7 +100,13 @@ int mmc_gpio_request_ro(struct mmc_host *host, unsigned int gpio)
 
         ctx = host->slot.handler_priv;
 
-        return gpio_request_one(gpio, GPIOF_DIR_IN, ctx->ro_label);
+        ret = gpio_request_one(gpio, GPIOF_DIR_IN, ctx->ro_label);
+        if (ret < 0)
+                return ret;
+
+        ctx->ro_gpio = gpio;
+
+        return 0;
 }
 EXPORT_SYMBOL(mmc_gpio_request_ro);
 
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index aa131b32e3b2..9bf10e7bbfaf 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -540,6 +540,15 @@ config MMC_DW_PLTFM
 
           If unsure, say Y.
 
+config MMC_DW_EXYNOS
+        tristate "Exynos specific extentions for Synopsys DW Memory Card Interface"
+        depends on MMC_DW
+        select MMC_DW_PLTFM
+        help
+          This selects support for Samsung Exynos SoC specific extensions to the
+          Synopsys DesignWare Memory Card Interface driver. Select this option
+          for platforms based on Exynos4 and Exynos5 SoC's.
+
 config MMC_DW_PCI
         tristate "Synopsys Designware MCI support on PCI bus"
         depends on MMC_DW && PCI
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 8922b06be925..17ad0a7ba40b 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -39,6 +39,7 @@ obj-$(CONFIG_MMC_VIA_SDMMC)	+= via-sdmmc.o
 obj-$(CONFIG_SDH_BFIN)          += bfin_sdh.o
 obj-$(CONFIG_MMC_DW)            += dw_mmc.o
 obj-$(CONFIG_MMC_DW_PLTFM)      += dw_mmc-pltfm.o
+obj-$(CONFIG_MMC_DW_EXYNOS)     += dw_mmc-exynos.o
 obj-$(CONFIG_MMC_DW_PCI)        += dw_mmc-pci.o
 obj-$(CONFIG_MMC_SH_MMCIF)      += sh_mmcif.o
 obj-$(CONFIG_MMC_JZ4740)        += jz4740_mmc.o
diff --git a/drivers/mmc/host/atmel-mci-regs.h b/drivers/mmc/host/atmel-mci-regs.h
index ab56f7db5315..c97001e15227 100644
--- a/drivers/mmc/host/atmel-mci-regs.h
+++ b/drivers/mmc/host/atmel-mci-regs.h
@@ -140,6 +140,13 @@
 #define atmci_writel(port,reg,value)                    \
         __raw_writel((value), (port)->regs + reg)
 
+/* On AVR chips the Peripheral DMA Controller is not connected to MCI. */
+#ifdef CONFIG_AVR32
+#       define ATMCI_PDC_CONNECTED      0
+#else
+#       define ATMCI_PDC_CONNECTED      1
+#endif
+
 /*
  * Fix sconfig's burst size according to atmel MCI. We need to convert them as:
  * 1 -> 0, 4 -> 1, 8 -> 2, 16 -> 3.
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 852d5fbda630..ddf096e3803f 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -19,6 +19,9 @@
 #include <linux/interrupt.h>
 #include <linux/ioport.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/scatterlist.h>
 #include <linux/seq_file.h>
@@ -71,7 +74,7 @@ enum atmci_pdc_buf {
 };
 
 struct atmel_mci_caps {
-        bool    has_dma;
+        bool    has_dma_conf_reg;
         bool    has_pdc;
         bool    has_cfg_reg;
         bool    has_cstor_reg;
@@ -418,7 +421,7 @@ static int atmci_regs_show(struct seq_file *s, void *v)
         atmci_show_status_reg(s, "SR", buf[ATMCI_SR / 4]);
         atmci_show_status_reg(s, "IMR", buf[ATMCI_IMR / 4]);
 
-        if (host->caps.has_dma) {
+        if (host->caps.has_dma_conf_reg) {
                 u32 val;
 
                 val = buf[ATMCI_DMA / 4];
@@ -500,6 +503,70 @@ err:
         dev_err(&mmc->class_dev, "failed to initialize debugfs for slot\n");
 }
 
+#if defined(CONFIG_OF)
+static const struct of_device_id atmci_dt_ids[] = {
+        { .compatible = "atmel,hsmci" },
+        { /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, atmci_dt_ids);
+
+static struct mci_platform_data __devinit*
+atmci_of_init(struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct device_node *cnp;
+        struct mci_platform_data *pdata;
+        u32 slot_id;
+
+        if (!np) {
+                dev_err(&pdev->dev, "device node not found\n");
+                return ERR_PTR(-EINVAL);
+        }
+
+        pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+        if (!pdata) {
+                dev_err(&pdev->dev, "could not allocate memory for pdata\n");
+                return ERR_PTR(-ENOMEM);
+        }
+
+        for_each_child_of_node(np, cnp) {
+                if (of_property_read_u32(cnp, "reg", &slot_id)) {
+                        dev_warn(&pdev->dev, "reg property is missing for %s\n",
+                                 cnp->full_name);
+                        continue;
+                }
+
+                if (slot_id >= ATMCI_MAX_NR_SLOTS) {
+                        dev_warn(&pdev->dev, "can't have more than %d slots\n",
+                                 ATMCI_MAX_NR_SLOTS);
+                        break;
+                }
+
+                if (of_property_read_u32(cnp, "bus-width",
+                                         &pdata->slot[slot_id].bus_width))
+                        pdata->slot[slot_id].bus_width = 1;
+
+                pdata->slot[slot_id].detect_pin =
+                        of_get_named_gpio(cnp, "cd-gpios", 0);
+
+                pdata->slot[slot_id].detect_is_active_high =
+                        of_property_read_bool(cnp, "cd-inverted");
+
+                pdata->slot[slot_id].wp_pin =
+                        of_get_named_gpio(cnp, "wp-gpios", 0);
+        }
+
+        return pdata;
+}
+#else /* CONFIG_OF */
+static inline struct mci_platform_data*
+atmci_of_init(struct platform_device *dev)
+{
+        return ERR_PTR(-EINVAL);
+}
+#endif
+
 static inline unsigned int atmci_get_version(struct atmel_mci *host)
 {
         return atmci_readl(host, ATMCI_VERSION) & 0x00000fff;
@@ -774,7 +841,7 @@ static void atmci_dma_complete(void *arg)
 
         dev_vdbg(&host->pdev->dev, "DMA complete\n");
 
-        if (host->caps.has_dma)
+        if (host->caps.has_dma_conf_reg)
                 /* Disable DMA hardware handshaking on MCI */
                 atmci_writel(host, ATMCI_DMA, atmci_readl(host, ATMCI_DMA) & ~ATMCI_DMAEN);
 
@@ -961,7 +1028,9 @@ atmci_prepare_data_dma(struct atmel_mci *host, struct mmc_data *data)
                 maxburst = atmci_convert_chksize(host->dma_conf.dst_maxburst);
         }
 
-        atmci_writel(host, ATMCI_DMA, ATMCI_DMA_CHKSIZE(maxburst) | ATMCI_DMAEN);
+        if (host->caps.has_dma_conf_reg)
+                atmci_writel(host, ATMCI_DMA, ATMCI_DMA_CHKSIZE(maxburst) |
+                        ATMCI_DMAEN);
 
         sglen = dma_map_sg(chan->device->dev, data->sg,
                         data->sg_len, direction);
@@ -2046,6 +2115,13 @@ static int __init atmci_init_slot(struct atmel_mci *host,
         slot->sdc_reg = sdc_reg;
         slot->sdio_irq = sdio_irq;
 
+        dev_dbg(&mmc->class_dev,
+                "slot[%u]: bus_width=%u, detect_pin=%d, "
+                "detect_is_active_high=%s, wp_pin=%d\n",
+                id, slot_data->bus_width, slot_data->detect_pin,
+                slot_data->detect_is_active_high ? "true" : "false",
+                slot_data->wp_pin);
+
         mmc->ops = &atmci_ops;
         mmc->f_min = DIV_ROUND_UP(host->bus_hz, 512);
         mmc->f_max = host->bus_hz / 2;
@@ -2169,7 +2245,10 @@ static bool atmci_configure_dma(struct atmel_mci *host)
 
         pdata = host->pdev->dev.platform_data;
 
-        if (pdata && find_slave_dev(pdata->dma_slave)) {
+        if (!pdata)
+                return false;
+
+        if (pdata->dma_slave && find_slave_dev(pdata->dma_slave)) {
                 dma_cap_mask_t mask;
 
                 /* Try to grab a DMA channel */
@@ -2210,8 +2289,8 @@ static void __init atmci_get_cap(struct atmel_mci *host)
         dev_info(&host->pdev->dev,
                         "version: 0x%x\n", version);
 
-        host->caps.has_dma = 0;
-        host->caps.has_pdc = 1;
+        host->caps.has_dma_conf_reg = 0;
+        host->caps.has_pdc = ATMCI_PDC_CONNECTED;
         host->caps.has_cfg_reg = 0;
         host->caps.has_cstor_reg = 0;
         host->caps.has_highspeed = 0;
@@ -2228,12 +2307,7 @@ static void __init atmci_get_cap(struct atmel_mci *host)
                 host->caps.has_odd_clk_div = 1;
         case 0x400:
         case 0x300:
-#ifdef CONFIG_AT_HDMAC
-                host->caps.has_dma = 1;
-#else
-                dev_info(&host->pdev->dev,
-                        "has dma capability but dma engine is not selected, then use pio\n");
-#endif
+                host->caps.has_dma_conf_reg = 1;
                 host->caps.has_pdc = 0;
                 host->caps.has_cfg_reg = 1;
                 host->caps.has_cstor_reg = 1;
@@ -2268,8 +2342,14 @@ static int __init atmci_probe(struct platform_device *pdev)
         if (!regs)
                 return -ENXIO;
         pdata = pdev->dev.platform_data;
-        if (!pdata)
-                return -ENXIO;
+        if (!pdata) {
+                pdata = atmci_of_init(pdev);
+                if (IS_ERR(pdata)) {
+                        dev_err(&pdev->dev, "platform data not available\n");
+                        return PTR_ERR(pdata);
+                }
+        }
+
         irq = platform_get_irq(pdev, 0);
         if (irq < 0)
                 return irq;
@@ -2308,7 +2388,7 @@ static int __init atmci_probe(struct platform_device *pdev)
 
         /* Get MCI capabilities and set operations according to it */
         atmci_get_cap(host);
-        if (host->caps.has_dma && atmci_configure_dma(host)) {
+        if (atmci_configure_dma(host)) {
                 host->prepare_data = &atmci_prepare_data_dma;
                 host->submit_data = &atmci_submit_data_dma;
                 host->stop_transfer = &atmci_stop_transfer_dma;
@@ -2487,6 +2567,7 @@ static struct platform_driver atmci_driver = {
         .driver         = {
                 .name           = "atmel_mci",
                 .pm             = ATMCI_PM_OPS,
+                .of_match_table = of_match_ptr(atmci_dt_ids),
         },
 };
 
diff --git a/drivers/mmc/host/bfin_sdh.c b/drivers/mmc/host/bfin_sdh.c
index a17dd7363ceb..b9b463eca1ec 100644
--- a/drivers/mmc/host/bfin_sdh.c
+++ b/drivers/mmc/host/bfin_sdh.c
@@ -24,9 +24,7 @@
 #include <asm/portmux.h>
 #include <asm/bfin_sdh.h>
 
-#if defined(CONFIG_BF51x)
-#define bfin_read_SDH_PWR_CTL           bfin_read_RSI_PWR_CTL
-#define bfin_write_SDH_PWR_CTL          bfin_write_RSI_PWR_CTL
+#if defined(CONFIG_BF51x) || defined(__ADSPBF60x__)
 #define bfin_read_SDH_CLK_CTL           bfin_read_RSI_CLK_CTL
 #define bfin_write_SDH_CLK_CTL          bfin_write_RSI_CLK_CTL
 #define bfin_write_SDH_ARGUMENT         bfin_write_RSI_ARGUMENT
@@ -45,8 +43,16 @@
 #define bfin_write_SDH_E_STATUS         bfin_write_RSI_E_STATUS
 #define bfin_read_SDH_STATUS            bfin_read_RSI_STATUS
 #define bfin_write_SDH_MASK0            bfin_write_RSI_MASK0
+#define bfin_write_SDH_E_MASK           bfin_write_RSI_E_MASK
 #define bfin_read_SDH_CFG               bfin_read_RSI_CFG
 #define bfin_write_SDH_CFG              bfin_write_RSI_CFG
+# if defined(__ADSPBF60x__)
+#  define bfin_read_SDH_BLK_SIZE        bfin_read_RSI_BLKSZ
+#  define bfin_write_SDH_BLK_SIZE       bfin_write_RSI_BLKSZ
+# else
+#  define bfin_read_SDH_PWR_CTL         bfin_read_RSI_PWR_CTL
+#  define bfin_write_SDH_PWR_CTL        bfin_write_RSI_PWR_CTL
+# endif
 #endif
 
 struct sdh_host {
@@ -62,6 +68,7 @@ struct sdh_host {
         dma_addr_t              sg_dma;
         int                     dma_len;
 
+        unsigned long           sclk;
         unsigned int            imask;
         unsigned int            power_mode;
         unsigned int            clk_div;
@@ -127,11 +134,15 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
         /* Only supports power-of-2 block size */
         if (data->blksz & (data->blksz - 1))
                 return -EINVAL;
+#ifndef RSI_BLKSZ
         data_ctl |= ((ffs(data->blksz) - 1) << 4);
+#else
+        bfin_write_SDH_BLK_SIZE(data->blksz);
+#endif
 
         bfin_write_SDH_DATA_CTL(data_ctl);
         /* the time of a host clock period in ns */
-        cycle_ns = 1000000000 / (get_sclk() / (2 * (host->clk_div + 1)));
+        cycle_ns = 1000000000 / (host->sclk / (2 * (host->clk_div + 1)));
         timeout = data->timeout_ns / cycle_ns;
         timeout += data->timeout_clks;
         bfin_write_SDH_DATA_TIMER(timeout);
@@ -145,8 +156,13 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
 
         sdh_enable_stat_irq(host, (DAT_CRC_FAIL | DAT_TIME_OUT | DAT_END));
         host->dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len, host->dma_dir);
-#if defined(CONFIG_BF54x)
-        dma_cfg |= DMAFLOW_ARRAY | NDSIZE_5 | RESTART | WDSIZE_32 | DMAEN;
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF60x)
+        dma_cfg |= DMAFLOW_ARRAY | RESTART | WDSIZE_32 | DMAEN;
+# ifdef RSI_BLKSZ
+        dma_cfg |= PSIZE_32 | NDSIZE_3;
+# else
+        dma_cfg |= NDSIZE_5;
+# endif
         {
                 struct scatterlist *sg;
                 int i;
@@ -156,7 +172,7 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
                         host->sg_cpu[i].x_count = sg_dma_len(sg) / 4;
                         host->sg_cpu[i].x_modify = 4;
                         dev_dbg(mmc_dev(host->mmc), "%d: start_addr:0x%lx, "
-                                "cfg:0x%x, x_count:0x%x, x_modify:0x%x\n",
+                                "cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
                                 i, host->sg_cpu[i].start_addr,
                                 host->sg_cpu[i].cfg, host->sg_cpu[i].x_count,
                                 host->sg_cpu[i].x_modify);
@@ -172,6 +188,7 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
         set_dma_curr_desc_addr(host->dma_ch, (unsigned long *)host->sg_dma);
         set_dma_x_count(host->dma_ch, 0);
         set_dma_x_modify(host->dma_ch, 0);
+        SSYNC();
         set_dma_config(host->dma_ch, dma_cfg);
 #elif defined(CONFIG_BF51x)
         /* RSI DMA doesn't work in array mode */
@@ -179,6 +196,7 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
         set_dma_start_addr(host->dma_ch, sg_dma_address(&data->sg[0]));
         set_dma_x_count(host->dma_ch, length / 4);
         set_dma_x_modify(host->dma_ch, 4);
+        SSYNC();
         set_dma_config(host->dma_ch, dma_cfg);
 #endif
         bfin_write_SDH_DATA_CTL(bfin_read_SDH_DATA_CTL() | DTX_DMA_E | DTX_E);
@@ -296,7 +314,6 @@ static int sdh_data_done(struct sdh_host *host, unsigned int stat)
         else
                 data->bytes_xfered = 0;
 
-        sdh_disable_stat_irq(host, DAT_END | DAT_TIME_OUT | DAT_CRC_FAIL | RX_OVERRUN | TX_UNDERRUN);
         bfin_write_SDH_STATUS_CLR(DAT_END_STAT | DAT_TIMEOUT_STAT | \
                         DAT_CRC_FAIL_STAT | DAT_BLK_END_STAT | RX_OVERRUN | TX_UNDERRUN);
         bfin_write_SDH_DATA_CTL(0);
@@ -321,74 +338,115 @@ static void sdh_request(struct mmc_host *mmc, struct mmc_request *mrq)
         dev_dbg(mmc_dev(host->mmc), "%s enter, mrp:%p, cmd:%p\n", __func__, mrq, mrq->cmd);
         WARN_ON(host->mrq != NULL);
 
+        spin_lock(&host->lock);
         host->mrq = mrq;
         host->data = mrq->data;
 
         if (mrq->data && mrq->data->flags & MMC_DATA_READ) {
                 ret = sdh_setup_data(host, mrq->data);
                 if (ret)
-                        return;
+                        goto data_err;
         }
 
         sdh_start_cmd(host, mrq->cmd);
+data_err:
+        spin_unlock(&host->lock);
 }
 
 static void sdh_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
         struct sdh_host *host;
-        unsigned long flags;
         u16 clk_ctl = 0;
+#ifndef RSI_BLKSZ
         u16 pwr_ctl = 0;
+#endif
         u16 cfg;
         host = mmc_priv(mmc);
 
-        spin_lock_irqsave(&host->lock, flags);
-        if (ios->clock) {
-                unsigned long  sys_clk, ios_clk;
-                unsigned char clk_div;
-                ios_clk = 2 * ios->clock;
-                sys_clk = get_sclk();
-                clk_div = sys_clk / ios_clk;
-                if (sys_clk % ios_clk == 0)
-                        clk_div -= 1;
-                clk_div = min_t(unsigned char, clk_div, 0xFF);
-                clk_ctl |= clk_div;
-                clk_ctl |= CLK_E;
-                host->clk_div = clk_div;
-        } else
-                sdh_stop_clock(host);
-
-        if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN)
-#ifdef CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
-                pwr_ctl |= ROD_CTL;
-#else
-                pwr_ctl |= SD_CMD_OD | ROD_CTL;
-#endif
+        spin_lock(&host->lock);
 
-        if (ios->bus_width == MMC_BUS_WIDTH_4) {
-                cfg = bfin_read_SDH_CFG();
+        cfg = bfin_read_SDH_CFG();
+        cfg |= MWE;
+        switch (ios->bus_width) {
+        case MMC_BUS_WIDTH_4:
+#ifndef RSI_BLKSZ
                 cfg &= ~PD_SDDAT3;
+#endif
                 cfg |= PUP_SDDAT3;
                 /* Enable 4 bit SDIO */
-                cfg |= (SD4E | MWE);
-                bfin_write_SDH_CFG(cfg);
-                clk_ctl |= WIDE_BUS;
-        } else {
-                cfg = bfin_read_SDH_CFG();
-                cfg |= MWE;
-                bfin_write_SDH_CFG(cfg);
+                cfg |= SD4E;
+                clk_ctl |= WIDE_BUS_4;
+                break;
+        case MMC_BUS_WIDTH_8:
+#ifndef RSI_BLKSZ
+                cfg &= ~PD_SDDAT3;
+#endif
+                cfg |= PUP_SDDAT3;
+                /* Disable 4 bit SDIO */
+                cfg &= ~SD4E;
+                clk_ctl |= BYTE_BUS_8;
+                break;
+        default:
+                cfg &= ~PUP_SDDAT3;
+                /* Disable 4 bit SDIO */
+                cfg &= ~SD4E;
         }
 
-        bfin_write_SDH_CLK_CTL(clk_ctl);
-
         host->power_mode = ios->power_mode;
-        if (ios->power_mode == MMC_POWER_ON)
+#ifndef RSI_BLKSZ
+        if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN) {
+                pwr_ctl |= ROD_CTL;
+# ifndef CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
+                pwr_ctl |= SD_CMD_OD;
+# endif
+        }
+
+        if (ios->power_mode != MMC_POWER_OFF)
                 pwr_ctl |= PWR_ON;
+        else
+                pwr_ctl &= ~PWR_ON;
 
         bfin_write_SDH_PWR_CTL(pwr_ctl);
+#else
+# ifndef CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
+        if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN)
+                cfg |= SD_CMD_OD;
+        else
+                cfg &= ~SD_CMD_OD;
+# endif
+
+
+        if (ios->power_mode != MMC_POWER_OFF)
+                cfg |= PWR_ON;
+        else
+                cfg &= ~PWR_ON;
+
+        bfin_write_SDH_CFG(cfg);
+#endif
         SSYNC();
 
-        spin_unlock_irqrestore(&host->lock, flags);
+        if (ios->power_mode == MMC_POWER_ON && ios->clock) {
+                unsigned char clk_div;
+                clk_div = (get_sclk() / ios->clock - 1) / 2;
+                clk_div = min_t(unsigned char, clk_div, 0xFF);
+                clk_ctl |= clk_div;
+                clk_ctl |= CLK_E;
+                host->clk_div = clk_div;
+                bfin_write_SDH_CLK_CTL(clk_ctl);
+
+        } else
+                sdh_stop_clock(host);
+
+        /* set up sdh interrupt mask*/
+        if (ios->power_mode == MMC_POWER_ON)
+                bfin_write_SDH_MASK0(DAT_END | DAT_TIME_OUT | DAT_CRC_FAIL |
+                        RX_OVERRUN | TX_UNDERRUN | CMD_SENT | CMD_RESP_END |
+                        CMD_TIME_OUT | CMD_CRC_FAIL);
+        else
+                bfin_write_SDH_MASK0(0);
+        SSYNC();
+
+        spin_unlock(&host->lock);
 
         dev_dbg(mmc_dev(host->mmc), "SDH: clk_div = 0x%x actual clock:%ld expected clock:%d\n",
                 host->clk_div,
@@ -405,7 +463,7 @@ static irqreturn_t sdh_dma_irq(int irq, void *devid)
 {
         struct sdh_host *host = devid;
 
-        dev_dbg(mmc_dev(host->mmc), "%s enter, irq_stat: 0x%04x\n", __func__,
+        dev_dbg(mmc_dev(host->mmc), "%s enter, irq_stat: 0x%04lx\n", __func__,
                 get_dma_curr_irqstat(host->dma_ch));
         clear_dma_irqstat(host->dma_ch);
         SSYNC();
@@ -420,6 +478,9 @@ static irqreturn_t sdh_stat_irq(int irq, void *devid)
         int handled = 0;
 
         dev_dbg(mmc_dev(host->mmc), "%s enter\n", __func__);
+
+        spin_lock(&host->lock);
+
         status = bfin_read_SDH_E_STATUS();
         if (status & SD_CARD_DET) {
                 mmc_detect_change(host->mmc, 0);
@@ -437,11 +498,30 @@ static irqreturn_t sdh_stat_irq(int irq, void *devid)
         if (status & (DAT_END | DAT_TIME_OUT | DAT_CRC_FAIL | RX_OVERRUN | TX_UNDERRUN))
                 handled |= sdh_data_done(host, status);
 
+        spin_unlock(&host->lock);
+
         dev_dbg(mmc_dev(host->mmc), "%s exit\n\n", __func__);
 
         return IRQ_RETVAL(handled);
 }
 
+static void sdh_reset(void)
+{
+#if defined(CONFIG_BF54x)
+        /* Secure Digital Host shares DMA with Nand controller */
+        bfin_write_DMAC1_PERIMUX(bfin_read_DMAC1_PERIMUX() | 0x1);
+#endif
+
+        bfin_write_SDH_CFG(bfin_read_SDH_CFG() | CLKS_EN);
+        SSYNC();
+
+        /* Disable card inserting detection pin. set MMC_CAP_NEEDS_POLL, and
+         * mmc stack will do the detection.
+         */
+        bfin_write_SDH_CFG((bfin_read_SDH_CFG() & 0x1F) | (PUP_SDDAT | PUP_SDDAT3));
+        SSYNC();
+}
+
 static int __devinit sdh_probe(struct platform_device *pdev)
 {
         struct mmc_host *mmc;
@@ -462,8 +542,16 @@ static int __devinit sdh_probe(struct platform_device *pdev)
         }
 
         mmc->ops = &sdh_ops;
-        mmc->max_segs = 32;
+#if defined(CONFIG_BF51x)
+        mmc->max_segs = 1;
+#else
+        mmc->max_segs = PAGE_SIZE / sizeof(struct dma_desc_array);
+#endif
+#ifdef RSI_BLKSZ
+        mmc->max_seg_size = -1;
+#else
         mmc->max_seg_size = 1 << 16;
+#endif
         mmc->max_blk_size = 1 << 11;
         mmc->max_blk_count = 1 << 11;
         mmc->max_req_size = PAGE_SIZE;
@@ -473,6 +561,7 @@ static int __devinit sdh_probe(struct platform_device *pdev)
         mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_NEEDS_POLL;
         host = mmc_priv(mmc);
         host->mmc = mmc;
+        host->sclk = get_sclk();
 
         spin_lock_init(&host->lock);
         host->irq = drv_data->irq_int0;
@@ -497,7 +586,6 @@ static int __devinit sdh_probe(struct platform_device *pdev)
         }
 
         platform_set_drvdata(pdev, mmc);
-        mmc_add_host(mmc);
 
         ret = request_irq(host->irq, sdh_stat_irq, 0, "SDH Status IRQ", host);
         if (ret) {
@@ -510,20 +598,10 @@ static int __devinit sdh_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "unable to request peripheral pins\n");
                 goto out4;
         }
-#if defined(CONFIG_BF54x)
-        /* Secure Digital Host shares DMA with Nand controller */
-        bfin_write_DMAC1_PERIMUX(bfin_read_DMAC1_PERIMUX() | 0x1);
-#endif
-
-        bfin_write_SDH_CFG(bfin_read_SDH_CFG() | CLKS_EN);
-        SSYNC();
 
-        /* Disable card inserting detection pin. set MMC_CAP_NEES_POLL, and
-         * mmc stack will do the detection.
-         */
-        bfin_write_SDH_CFG((bfin_read_SDH_CFG() & 0x1F) | (PUP_SDDAT | PUP_SDDAT3));
-        SSYNC();
+        sdh_reset();
 
+        mmc_add_host(mmc);
         return 0;
 
 out4:
@@ -571,7 +649,6 @@ static int sdh_suspend(struct platform_device *dev, pm_message_t state)
         if (mmc)
                 ret = mmc_suspend_host(mmc);
 
-        bfin_write_SDH_PWR_CTL(bfin_read_SDH_PWR_CTL() & ~PWR_ON);
         peripheral_free_list(drv_data->pin_req);
 
         return ret;
@@ -589,16 +666,7 @@ static int sdh_resume(struct platform_device *dev)
                 return ret;
         }
 
-        bfin_write_SDH_PWR_CTL(bfin_read_SDH_PWR_CTL() | PWR_ON);
-#if defined(CONFIG_BF54x)
-        /* Secure Digital Host shares DMA with Nand controller */
-        bfin_write_DMAC1_PERIMUX(bfin_read_DMAC1_PERIMUX() | 0x1);
-#endif
-        bfin_write_SDH_CFG(bfin_read_SDH_CFG() | CLKS_EN);
-        SSYNC();
-
-        bfin_write_SDH_CFG((bfin_read_SDH_CFG() & 0x1F) | (PUP_SDDAT | PUP_SDDAT3));
-        SSYNC();
+        sdh_reset();
 
         if (mmc)
                 ret = mmc_resume_host(mmc);
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index 3dfd3473269d..20636772c09b 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -30,11 +30,12 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/delay.h>
+#include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
+#include <linux/edma.h>
 #include <linux/mmc/mmc.h>
 
 #include <linux/platform_data/mmc-davinci.h>
-#include <mach/edma.h>
 
 /*
  * Register Definitions
@@ -200,21 +201,13 @@ struct mmc_davinci_host {
         u32 bytes_left;
 
         u32 rxdma, txdma;
+        struct dma_chan *dma_tx;
+        struct dma_chan *dma_rx;
         bool use_dma;
         bool do_dma;
         bool sdio_int;
         bool active_request;
 
-        /* Scatterlist DMA uses one or more parameter RAM entries:
-         * the main one (associated with rxdma or txdma) plus zero or
-         * more links.  The entries for a given transfer differ only
-         * by memory buffer (address, length) and link field.
-         */
-        struct edmacc_param     tx_template;
-        struct edmacc_param     rx_template;
-        unsigned                n_link;
-        u32                     links[MAX_NR_SG - 1];
-
         /* For PIO we walk scatterlists one segment at a time. */
         unsigned int            sg_len;
         struct scatterlist *sg;
@@ -410,153 +403,74 @@ static void mmc_davinci_start_command(struct mmc_davinci_host *host,
 
 static void davinci_abort_dma(struct mmc_davinci_host *host)
 {
-        int sync_dev;
+        struct dma_chan *sync_dev;
 
         if (host->data_dir == DAVINCI_MMC_DATADIR_READ)
-                sync_dev = host->rxdma;
+                sync_dev = host->dma_rx;
         else
-                sync_dev = host->txdma;
-
-        edma_stop(sync_dev);
-        edma_clean_channel(sync_dev);
-}
-
-static void
-mmc_davinci_xfer_done(struct mmc_davinci_host *host, struct mmc_data *data);
-
-static void mmc_davinci_dma_cb(unsigned channel, u16 ch_status, void *data)
-{
-        if (DMA_COMPLETE != ch_status) {
-                struct mmc_davinci_host *host = data;
-
-                /* Currently means:  DMA Event Missed, or "null" transfer
-                 * request was seen.  In the future, TC errors (like bad
-                 * addresses) might be presented too.
-                 */
-                dev_warn(mmc_dev(host->mmc), "DMA %s error\n",
-                        (host->data->flags & MMC_DATA_WRITE)
-                                ? "write" : "read");
-                host->data->error = -EIO;
-                mmc_davinci_xfer_done(host, host->data);
-        }
-}
-
-/* Set up tx or rx template, to be modified and updated later */
-static void __init mmc_davinci_dma_setup(struct mmc_davinci_host *host,
-                bool tx, struct edmacc_param *template)
-{
-        unsigned        sync_dev;
-        const u16       acnt = 4;
-        const u16       bcnt = rw_threshold >> 2;
-        const u16       ccnt = 0;
-        u32             src_port = 0;
-        u32             dst_port = 0;
-        s16             src_bidx, dst_bidx;
-        s16             src_cidx, dst_cidx;
-
-        /*
-         * A-B Sync transfer:  each DMA request is for one "frame" of
-         * rw_threshold bytes, broken into "acnt"-size chunks repeated
-         * "bcnt" times.  Each segment needs "ccnt" such frames; since
-         * we tell the block layer our mmc->max_seg_size limit, we can
-         * trust (later) that it's within bounds.
-         *
-         * The FIFOs are read/written in 4-byte chunks (acnt == 4) and
-         * EDMA will optimize memory operations to use larger bursts.
-         */
-        if (tx) {
-                sync_dev = host->txdma;
-
-                /* src_prt, ccnt, and link to be set up later */
-                src_bidx = acnt;
-                src_cidx = acnt * bcnt;
-
-                dst_port = host->mem_res->start + DAVINCI_MMCDXR;
-                dst_bidx = 0;
-                dst_cidx = 0;
-        } else {
-                sync_dev = host->rxdma;
-
-                src_port = host->mem_res->start + DAVINCI_MMCDRR;
-                src_bidx = 0;
-                src_cidx = 0;
-
-                /* dst_prt, ccnt, and link to be set up later */
-                dst_bidx = acnt;
-                dst_cidx = acnt * bcnt;
-        }
-
-        /*
-         * We can't use FIFO mode for the FIFOs because MMC FIFO addresses
-         * are not 256-bit (32-byte) aligned.  So we use INCR, and the W8BIT
-         * parameter is ignored.
-         */
-        edma_set_src(sync_dev, src_port, INCR, W8BIT);
-        edma_set_dest(sync_dev, dst_port, INCR, W8BIT);
+                sync_dev = host->dma_tx;
 
-        edma_set_src_index(sync_dev, src_bidx, src_cidx);
-        edma_set_dest_index(sync_dev, dst_bidx, dst_cidx);
-
-        edma_set_transfer_params(sync_dev, acnt, bcnt, ccnt, 8, ABSYNC);
-
-        edma_read_slot(sync_dev, template);
-
-        /* don't bother with irqs or chaining */
-        template->opt |= EDMA_CHAN_SLOT(sync_dev) << 12;
+        dmaengine_terminate_all(sync_dev);
 }
 
-static void mmc_davinci_send_dma_request(struct mmc_davinci_host *host,
+static int mmc_davinci_send_dma_request(struct mmc_davinci_host *host,
                 struct mmc_data *data)
 {
-        struct edmacc_param     *template;
-        int                     channel, slot;
-        unsigned                link;
-        struct scatterlist      *sg;
-        unsigned                sg_len;
-        unsigned                bytes_left = host->bytes_left;
-        const unsigned          shift = ffs(rw_threshold) - 1;
+        struct dma_chan *chan;
+        struct dma_async_tx_descriptor *desc;
+        int ret = 0;
 
         if (host->data_dir == DAVINCI_MMC_DATADIR_WRITE) {
-                template = &host->tx_template;
-                channel = host->txdma;
+                struct dma_slave_config dma_tx_conf = {
+                        .direction = DMA_MEM_TO_DEV,
+                        .dst_addr = host->mem_res->start + DAVINCI_MMCDXR,
+                        .dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+                        .dst_maxburst =
+                                rw_threshold / DMA_SLAVE_BUSWIDTH_4_BYTES,
+                };
+                chan = host->dma_tx;
+                dmaengine_slave_config(host->dma_tx, &dma_tx_conf);
+
+                desc = dmaengine_prep_slave_sg(host->dma_tx,
+                                data->sg,
+                                host->sg_len,
+                                DMA_MEM_TO_DEV,
+                                DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!desc) {
+                        dev_dbg(mmc_dev(host->mmc),
+                                "failed to allocate DMA TX descriptor");
+                        ret = -1;
+                        goto out;
+                }
         } else {
-                template = &host->rx_template;
-                channel = host->rxdma;
-        }
-
-        /* We know sg_len and ccnt will never be out of range because
-         * we told the mmc layer which in turn tells the block layer
-         * to ensure that it only hands us one scatterlist segment
-         * per EDMA PARAM entry.  Update the PARAM
-         * entries needed for each segment of this scatterlist.
-         */
-        for (slot = channel, link = 0, sg = data->sg, sg_len = host->sg_len;
-                        sg_len-- != 0 && bytes_left;
-                        sg = sg_next(sg), slot = host->links[link++]) {
-                u32             buf = sg_dma_address(sg);
-                unsigned        count = sg_dma_len(sg);
-
-                template->link_bcntrld = sg_len
-                                ? (EDMA_CHAN_SLOT(host->links[link]) << 5)
-                                : 0xffff;
-
-                if (count > bytes_left)
-                        count = bytes_left;
-                bytes_left -= count;
-
-                if (host->data_dir == DAVINCI_MMC_DATADIR_WRITE)
-                        template->src = buf;
-                else
-                        template->dst = buf;
-                template->ccnt = count >> shift;
-
-                edma_write_slot(slot, template);
+                struct dma_slave_config dma_rx_conf = {
+                        .direction = DMA_DEV_TO_MEM,
+                        .src_addr = host->mem_res->start + DAVINCI_MMCDRR,
+                        .src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+                        .src_maxburst =
+                                rw_threshold / DMA_SLAVE_BUSWIDTH_4_BYTES,
+                };
+                chan = host->dma_rx;
+                dmaengine_slave_config(host->dma_rx, &dma_rx_conf);
+
+                desc = dmaengine_prep_slave_sg(host->dma_rx,
+                                data->sg,
+                                host->sg_len,
+                                DMA_DEV_TO_MEM,
+                                DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!desc) {
+                        dev_dbg(mmc_dev(host->mmc),
+                                "failed to allocate DMA RX descriptor");
+                        ret = -1;
+                        goto out;
+                }
         }
 
-        if (host->version == MMC_CTLR_VERSION_2)
-                edma_clear_event(channel);
+        dmaengine_submit(desc);
+        dma_async_issue_pending(chan);
 
-        edma_start(channel);
+out:
+        return ret;
 }
 
 static int mmc_davinci_start_dma_transfer(struct mmc_davinci_host *host,
@@ -564,6 +478,7 @@ static int mmc_davinci_start_dma_transfer(struct mmc_davinci_host *host,
 {
         int i;
         int mask = rw_threshold - 1;
+        int ret = 0;
 
         host->sg_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
                                 ((data->flags & MMC_DATA_WRITE)
@@ -583,70 +498,48 @@ static int mmc_davinci_start_dma_transfer(struct mmc_davinci_host *host,
         }
 
         host->do_dma = 1;
-        mmc_davinci_send_dma_request(host, data);
+        ret = mmc_davinci_send_dma_request(host, data);
 
-        return 0;
+        return ret;
 }
 
 static void __init_or_module
 davinci_release_dma_channels(struct mmc_davinci_host *host)
 {
-        unsigned        i;
-
         if (!host->use_dma)
                 return;
 
-        for (i = 0; i < host->n_link; i++)
-                edma_free_slot(host->links[i]);
-
-        edma_free_channel(host->txdma);
-        edma_free_channel(host->rxdma);
+        dma_release_channel(host->dma_tx);
+        dma_release_channel(host->dma_rx);
 }
 
 static int __init davinci_acquire_dma_channels(struct mmc_davinci_host *host)
 {
-        u32 link_size;
-        int r, i;
-
-        /* Acquire master DMA write channel */
-        r = edma_alloc_channel(host->txdma, mmc_davinci_dma_cb, host,
-                        EVENTQ_DEFAULT);
-        if (r < 0) {
-                dev_warn(mmc_dev(host->mmc), "alloc %s channel err %d\n",
-                                "tx", r);
-                return r;
-        }
-        mmc_davinci_dma_setup(host, true, &host->tx_template);
-
-        /* Acquire master DMA read channel */
-        r = edma_alloc_channel(host->rxdma, mmc_davinci_dma_cb, host,
-                        EVENTQ_DEFAULT);
-        if (r < 0) {
-                dev_warn(mmc_dev(host->mmc), "alloc %s channel err %d\n",
-                                "rx", r);
-                goto free_master_write;
+        int r;
+        dma_cap_mask_t mask;
+
+        dma_cap_zero(mask);
+        dma_cap_set(DMA_SLAVE, mask);
+
+        host->dma_tx =
+                dma_request_channel(mask, edma_filter_fn, &host->txdma);
+        if (!host->dma_tx) {
+                dev_err(mmc_dev(host->mmc), "Can't get dma_tx channel\n");
+                return -ENODEV;
         }
-        mmc_davinci_dma_setup(host, false, &host->rx_template);
 
-        /* Allocate parameter RAM slots, which will later be bound to a
-         * channel as needed to handle a scatterlist.
-         */
-        link_size = min_t(unsigned, host->nr_sg, ARRAY_SIZE(host->links));
-        for (i = 0; i < link_size; i++) {
-                r = edma_alloc_slot(EDMA_CTLR(host->txdma), EDMA_SLOT_ANY);
-                if (r < 0) {
-                        dev_dbg(mmc_dev(host->mmc), "dma PaRAM alloc --> %d\n",
-                                r);
-                        break;
-                }
-                host->links[i] = r;
+        host->dma_rx =
+                dma_request_channel(mask, edma_filter_fn, &host->rxdma);
+        if (!host->dma_rx) {
+                dev_err(mmc_dev(host->mmc), "Can't get dma_rx channel\n");
+                r = -ENODEV;
+                goto free_master_write;
         }
-        host->n_link = i;
 
         return 0;
 
 free_master_write:
-        edma_free_channel(host->txdma);
+        dma_release_channel(host->dma_tx);
 
         return r;
 }
@@ -1359,7 +1252,7 @@ static int __init davinci_mmcsd_probe(struct platform_device *pdev)
          * Each hw_seg uses one EDMA parameter RAM slot, always one
          * channel and then usually some linked slots.
          */
-        mmc->max_segs           = 1 + host->n_link;
+        mmc->max_segs           = MAX_NR_SG;
 
         /* EDMA limit per hw segment (one or two MBytes) */
         mmc->max_seg_size       = MAX_CCNT * rw_threshold;
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
new file mode 100644
index 000000000000..660bbc528862
--- /dev/null
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -0,0 +1,253 @@
+/*
+ * Exynos Specific Extensions for Synopsys DW Multimedia Card Interface driver
+ *
+ * Copyright (C) 2012, Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/dw_mmc.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include "dw_mmc.h"
+#include "dw_mmc-pltfm.h"
+
+#define NUM_PINS(x)                     (x + 2)
+
+#define SDMMC_CLKSEL                    0x09C
+#define SDMMC_CLKSEL_CCLK_SAMPLE(x)     (((x) & 7) << 0)
+#define SDMMC_CLKSEL_CCLK_DRIVE(x)      (((x) & 7) << 16)
+#define SDMMC_CLKSEL_CCLK_DIVIDER(x)    (((x) & 7) << 24)
+#define SDMMC_CLKSEL_GET_DRV_WD3(x)     (((x) >> 16) & 0x7)
+#define SDMMC_CLKSEL_TIMING(x, y, z)    (SDMMC_CLKSEL_CCLK_SAMPLE(x) |  \
+                                        SDMMC_CLKSEL_CCLK_DRIVE(y) |    \
+                                        SDMMC_CLKSEL_CCLK_DIVIDER(z))
+
+#define SDMMC_CMD_USE_HOLD_REG          BIT(29)
+
+#define EXYNOS4210_FIXED_CIU_CLK_DIV    2
+#define EXYNOS4412_FIXED_CIU_CLK_DIV    4
+
+/* Variations in Exynos specific dw-mshc controller */
+enum dw_mci_exynos_type {
+        DW_MCI_TYPE_EXYNOS4210,
+        DW_MCI_TYPE_EXYNOS4412,
+        DW_MCI_TYPE_EXYNOS5250,
+};
+
+/* Exynos implementation specific driver private data */
+struct dw_mci_exynos_priv_data {
+        enum dw_mci_exynos_type         ctrl_type;
+        u8                              ciu_div;
+        u32                             sdr_timing;
+        u32                             ddr_timing;
+};
+
+static struct dw_mci_exynos_compatible {
+        char                            *compatible;
+        enum dw_mci_exynos_type         ctrl_type;
+} exynos_compat[] = {
+        {
+                .compatible     = "samsung,exynos4210-dw-mshc",
+                .ctrl_type      = DW_MCI_TYPE_EXYNOS4210,
+        }, {
+                .compatible     = "samsung,exynos4412-dw-mshc",
+                .ctrl_type      = DW_MCI_TYPE_EXYNOS4412,
+        }, {
+                .compatible     = "samsung,exynos5250-dw-mshc",
+                .ctrl_type      = DW_MCI_TYPE_EXYNOS5250,
+        },
+};
+
+static int dw_mci_exynos_priv_init(struct dw_mci *host)
+{
+        struct dw_mci_exynos_priv_data *priv;
+        int idx;
+
+        priv = devm_kzalloc(host->dev, sizeof(*priv), GFP_KERNEL);
+        if (!priv) {
+                dev_err(host->dev, "mem alloc failed for private data\n");
+                return -ENOMEM;
+        }
+
+        for (idx = 0; idx < ARRAY_SIZE(exynos_compat); idx++) {
+                if (of_device_is_compatible(host->dev->of_node,
+                                        exynos_compat[idx].compatible))
+                        priv->ctrl_type = exynos_compat[idx].ctrl_type;
+        }
+
+        host->priv = priv;
+        return 0;
+}
+
+static int dw_mci_exynos_setup_clock(struct dw_mci *host)
+{
+        struct dw_mci_exynos_priv_data *priv = host->priv;
+
+        if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS5250)
+                host->bus_hz /= (priv->ciu_div + 1);
+        else if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS4412)
+                host->bus_hz /= EXYNOS4412_FIXED_CIU_CLK_DIV;
+        else if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS4210)
+                host->bus_hz /= EXYNOS4210_FIXED_CIU_CLK_DIV;
+
+        return 0;
+}
+
+static void dw_mci_exynos_prepare_command(struct dw_mci *host, u32 *cmdr)
+{
+        /*
+         * Exynos4412 and Exynos5250 extends the use of CMD register with the
+         * use of bit 29 (which is reserved on standard MSHC controllers) for
+         * optionally bypassing the HOLD register for command and data. The
+         * HOLD register should be bypassed in case there is no phase shift
+         * applied on CMD/DATA that is sent to the card.
+         */
+        if (SDMMC_CLKSEL_GET_DRV_WD3(mci_readl(host, CLKSEL)))
+                *cmdr |= SDMMC_CMD_USE_HOLD_REG;
+}
+
+static void dw_mci_exynos_set_ios(struct dw_mci *host, struct mmc_ios *ios)
+{
+        struct dw_mci_exynos_priv_data *priv = host->priv;
+
+        if (ios->timing == MMC_TIMING_UHS_DDR50)
+                mci_writel(host, CLKSEL, priv->ddr_timing);
+        else
+                mci_writel(host, CLKSEL, priv->sdr_timing);
+}
+
+static int dw_mci_exynos_parse_dt(struct dw_mci *host)
+{
+        struct dw_mci_exynos_priv_data *priv = host->priv;
+        struct device_node *np = host->dev->of_node;
+        u32 timing[2];
+        u32 div = 0;
+        int ret;
+
+        of_property_read_u32(np, "samsung,dw-mshc-ciu-div", &div);
+        priv->ciu_div = div;
+
+        ret = of_property_read_u32_array(np,
+                        "samsung,dw-mshc-sdr-timing", timing, 2);
+        if (ret)
+                return ret;
+
+        priv->sdr_timing = SDMMC_CLKSEL_TIMING(timing[0], timing[1], div);
+
+        ret = of_property_read_u32_array(np,
+                        "samsung,dw-mshc-ddr-timing", timing, 2);
+        if (ret)
+                return ret;
+
+        priv->ddr_timing = SDMMC_CLKSEL_TIMING(timing[0], timing[1], div);
+        return 0;
+}
+
+static int dw_mci_exynos_setup_bus(struct dw_mci *host,
+                                struct device_node *slot_np, u8 bus_width)
+{
+        int idx, gpio, ret;
+
+        if (!slot_np)
+                return -EINVAL;
+
+        /* cmd + clock + bus-width pins */
+        for (idx = 0; idx < NUM_PINS(bus_width); idx++) {
+                gpio = of_get_gpio(slot_np, idx);
+                if (!gpio_is_valid(gpio)) {
+                        dev_err(host->dev, "invalid gpio: %d\n", gpio);
+                        return -EINVAL;
+                }
+
+                ret = devm_gpio_request(host->dev, gpio, "dw-mci-bus");
+                if (ret) {
+                        dev_err(host->dev, "gpio [%d] request failed\n", gpio);
+                        return -EBUSY;
+                }
+        }
+
+        gpio = of_get_named_gpio(slot_np, "wp-gpios", 0);
+        if (gpio_is_valid(gpio)) {
+                if (devm_gpio_request(host->dev, gpio, "dw-mci-wp"))
+                        dev_info(host->dev, "gpio [%d] request failed\n",
+                                                gpio);
+        } else {
+                dev_info(host->dev, "wp gpio not available");
+                host->pdata->quirks |= DW_MCI_QUIRK_NO_WRITE_PROTECT;
+        }
+
+        if (host->pdata->quirks & DW_MCI_QUIRK_BROKEN_CARD_DETECTION)
+                return 0;
+
+        gpio = of_get_named_gpio(slot_np, "samsung,cd-pinmux-gpio", 0);
+        if (gpio_is_valid(gpio)) {
+                if (devm_gpio_request(host->dev, gpio, "dw-mci-cd"))
+                        dev_err(host->dev, "gpio [%d] request failed\n", gpio);
+        } else {
+                dev_info(host->dev, "cd gpio not available");
+        }
+
+        return 0;
+}
+
+/* Exynos5250 controller specific capabilities */
+static unsigned long exynos5250_dwmmc_caps[4] = {
+        MMC_CAP_UHS_DDR50 | MMC_CAP_1_8V_DDR |
+                MMC_CAP_8_BIT_DATA | MMC_CAP_CMD23,
+        MMC_CAP_CMD23,
+        MMC_CAP_CMD23,
+        MMC_CAP_CMD23,
+};
+
+static struct dw_mci_drv_data exynos5250_drv_data = {
+        .caps                   = exynos5250_dwmmc_caps,
+        .init                   = dw_mci_exynos_priv_init,
+        .setup_clock            = dw_mci_exynos_setup_clock,
+        .prepare_command        = dw_mci_exynos_prepare_command,
+        .set_ios                = dw_mci_exynos_set_ios,
+        .parse_dt               = dw_mci_exynos_parse_dt,
+        .setup_bus              = dw_mci_exynos_setup_bus,
+};
+
+static const struct of_device_id dw_mci_exynos_match[] = {
+        { .compatible = "samsung,exynos5250-dw-mshc",
+                        .data = (void *)&exynos5250_drv_data, },
+        {},
+};
+MODULE_DEVICE_TABLE(of, dw_mci_pltfm_match);
+
+int dw_mci_exynos_probe(struct platform_device *pdev)
+{
+        struct dw_mci_drv_data *drv_data;
+        const struct of_device_id *match;
+
+        match = of_match_node(dw_mci_exynos_match, pdev->dev.of_node);
+        drv_data = match->data;
+        return dw_mci_pltfm_register(pdev, drv_data);
+}
+
+static struct platform_driver dw_mci_exynos_pltfm_driver = {
+        .probe          = dw_mci_exynos_probe,
+        .remove         = __exit_p(dw_mci_pltfm_remove),
+        .driver         = {
+                .name           = "dwmmc_exynos",
+                .of_match_table = of_match_ptr(dw_mci_exynos_match),
+                .pm             = &dw_mci_pltfm_pmops,
+        },
+};
+
+module_platform_driver(dw_mci_exynos_pltfm_driver);
+
+MODULE_DESCRIPTION("Samsung Specific DW-MSHC Driver Extension");
+MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:dwmmc-exynos");
diff --git a/drivers/mmc/host/dw_mmc-pci.c b/drivers/mmc/host/dw_mmc-pci.c
index dc0d25a013e0..edb37e9135ae 100644
--- a/drivers/mmc/host/dw_mmc-pci.c
+++ b/drivers/mmc/host/dw_mmc-pci.c
@@ -59,7 +59,7 @@ static int __devinit dw_mci_pci_probe(struct pci_dev *pdev,
 
         host->irq = pdev->irq;
         host->irq_flags = IRQF_SHARED;
-        host->dev = pdev->dev;
+        host->dev = &pdev->dev;
         host->pdata = &pci_board_data;
 
         host->regs = pci_iomap(pdev, PCI_BAR_NO, COMPLETE_BAR);
@@ -140,18 +140,7 @@ static struct pci_driver dw_mci_pci_driver = {
         },
 };
 
-static int __init dw_mci_init(void)
-{
-        return pci_register_driver(&dw_mci_pci_driver);
-}
-
-static void __exit dw_mci_exit(void)
-{
-        pci_unregister_driver(&dw_mci_pci_driver);
-}
-
-module_init(dw_mci_init);
-module_exit(dw_mci_exit);
+module_pci_driver(dw_mci_pci_driver);
 
 MODULE_DESCRIPTION("DW Multimedia Card PCI Interface driver");
 MODULE_AUTHOR("Shashidhar Hiremath <shashidharh@vayavyalabs.com>");
diff --git a/drivers/mmc/host/dw_mmc-pltfm.c b/drivers/mmc/host/dw_mmc-pltfm.c
index 92ec3eb3aae7..c960ca7ffbe6 100644
--- a/drivers/mmc/host/dw_mmc-pltfm.c
+++ b/drivers/mmc/host/dw_mmc-pltfm.c
@@ -19,59 +19,63 @@
 #include <linux/mmc/host.h>
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/dw_mmc.h>
+#include <linux/of.h>
+
 #include "dw_mmc.h"
 
-static int dw_mci_pltfm_probe(struct platform_device *pdev)
+int dw_mci_pltfm_register(struct platform_device *pdev,
+                                struct dw_mci_drv_data *drv_data)
 {
         struct dw_mci *host;
         struct resource *regs;
         int ret;
 
-        host = kzalloc(sizeof(struct dw_mci), GFP_KERNEL);
+        host = devm_kzalloc(&pdev->dev, sizeof(struct dw_mci), GFP_KERNEL);
         if (!host)
                 return -ENOMEM;
 
         regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!regs) {
-                ret = -ENXIO;
-                goto err_free;
-        }
+        if (!regs)
+                return -ENXIO;
 
         host->irq = platform_get_irq(pdev, 0);
-        if (host->irq < 0) {
-                ret = host->irq;
-                goto err_free;
-        }
+        if (host->irq < 0)
+                return host->irq;
 
-        host->dev = pdev->dev;
+        host->drv_data = drv_data;
+        host->dev = &pdev->dev;
         host->irq_flags = 0;
         host->pdata = pdev->dev.platform_data;
-        ret = -ENOMEM;
-        host->regs = ioremap(regs->start, resource_size(regs));
+        host->regs = devm_request_and_ioremap(&pdev->dev, regs);
         if (!host->regs)
-                goto err_free;
+                return -ENOMEM;
+
+        if (host->drv_data->init) {
+                ret = host->drv_data->init(host);
+                if (ret)
+                        return ret;
+        }
+
         platform_set_drvdata(pdev, host);
         ret = dw_mci_probe(host);
-        if (ret)
-                goto err_out;
-        return ret;
-err_out:
-        iounmap(host->regs);
-err_free:
-        kfree(host);
         return ret;
 }
+EXPORT_SYMBOL_GPL(dw_mci_pltfm_register);
 
-static int __exit dw_mci_pltfm_remove(struct platform_device *pdev)
+static int __devinit dw_mci_pltfm_probe(struct platform_device *pdev)
+{
+        return dw_mci_pltfm_register(pdev, NULL);
+}
+
+static int __devexit dw_mci_pltfm_remove(struct platform_device *pdev)
 {
         struct dw_mci *host = platform_get_drvdata(pdev);
 
         platform_set_drvdata(pdev, NULL);
         dw_mci_remove(host);
-        iounmap(host->regs);
-        kfree(host);
         return 0;
 }
+EXPORT_SYMBOL_GPL(dw_mci_pltfm_remove);
 
 #ifdef CONFIG_PM_SLEEP
 /*
@@ -105,12 +109,20 @@ static int dw_mci_pltfm_resume(struct device *dev)
 #define dw_mci_pltfm_resume     NULL
 #endif /* CONFIG_PM_SLEEP */
 
-static SIMPLE_DEV_PM_OPS(dw_mci_pltfm_pmops, dw_mci_pltfm_suspend, dw_mci_pltfm_resume);
+SIMPLE_DEV_PM_OPS(dw_mci_pltfm_pmops, dw_mci_pltfm_suspend, dw_mci_pltfm_resume);
+EXPORT_SYMBOL_GPL(dw_mci_pltfm_pmops);
+
+static const struct of_device_id dw_mci_pltfm_match[] = {
+        { .compatible = "snps,dw-mshc", },
+        {},
+};
+MODULE_DEVICE_TABLE(of, dw_mci_pltfm_match);
 
 static struct platform_driver dw_mci_pltfm_driver = {
         .remove         = __exit_p(dw_mci_pltfm_remove),
         .driver         = {
                 .name           = "dw_mmc",
+                .of_match_table = of_match_ptr(dw_mci_pltfm_match),
                 .pm             = &dw_mci_pltfm_pmops,
         },
 };
diff --git a/drivers/mmc/host/dw_mmc-pltfm.h b/drivers/mmc/host/dw_mmc-pltfm.h
new file mode 100644
index 000000000000..301f24541fc2
--- /dev/null
+++ b/drivers/mmc/host/dw_mmc-pltfm.h
@@ -0,0 +1,20 @@
+/*
+ * Synopsys DesignWare Multimedia Card Interface Platform driver
+ *
+ * Copyright (C) 2012, Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ */
+
+#ifndef _DW_MMC_PLTFM_H_
+#define _DW_MMC_PLTFM_H_
+
+extern int dw_mci_pltfm_register(struct platform_device *pdev,
+                                struct dw_mci_drv_data *drv_data);
+extern int __devexit dw_mci_pltfm_remove(struct platform_device *pdev);
+extern const struct dev_pm_ops dw_mci_pltfm_pmops;
+
+#endif /* _DW_MMC_PLTFM_H_ */
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index af40d227bece..c2828f35c3b8 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -33,6 +33,7 @@
 #include <linux/bitops.h>
 #include <linux/regulator/consumer.h>
 #include <linux/workqueue.h>
+#include <linux/of.h>
 
 #include "dw_mmc.h"
 
@@ -230,6 +231,7 @@ static void dw_mci_set_timeout(struct dw_mci *host)
 static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 {
         struct mmc_data *data;
+        struct dw_mci_slot *slot = mmc_priv(mmc);
         u32 cmdr;
         cmd->error = -EINPROGRESS;
 
@@ -259,6 +261,9 @@ static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
                         cmdr |= SDMMC_CMD_DAT_WR;
         }
 
+        if (slot->host->drv_data->prepare_command)
+                slot->host->drv_data->prepare_command(slot->host, &cmdr);
+
         return cmdr;
 }
 
@@ -266,7 +271,7 @@ static void dw_mci_start_command(struct dw_mci *host,
                                  struct mmc_command *cmd, u32 cmd_flags)
 {
         host->cmd = cmd;
-        dev_vdbg(&host->dev,
+        dev_vdbg(host->dev,
                  "start command: ARGR=0x%08x CMDR=0x%08x\n",
                  cmd->arg, cmd_flags);
 
@@ -308,7 +313,7 @@ static void dw_mci_dma_cleanup(struct dw_mci *host)
 
         if (data)
                 if (!data->host_cookie)
-                        dma_unmap_sg(&host->dev,
+                        dma_unmap_sg(host->dev,
                                      data->sg,
                                      data->sg_len,
                                      dw_mci_get_dma_dir(data));
@@ -334,7 +339,7 @@ static void dw_mci_idmac_complete_dma(struct dw_mci *host)
 {
         struct mmc_data *data = host->data;
 
-        dev_vdbg(&host->dev, "DMA complete\n");
+        dev_vdbg(host->dev, "DMA complete\n");
 
         host->dma_ops->cleanup(host);
 
@@ -405,23 +410,11 @@ static void dw_mci_idmac_start_dma(struct dw_mci *host, unsigned int sg_len)
 static int dw_mci_idmac_init(struct dw_mci *host)
 {
         struct idmac_desc *p;
-        int i, dma_support;
+        int i;
 
         /* Number of descriptors in the ring buffer */
         host->ring_size = PAGE_SIZE / sizeof(struct idmac_desc);
 
-        /* Check if Hardware Configuration Register has support for DMA */
-        dma_support = (mci_readl(host, HCON) >> 16) & 0x3;
-
-        if (!dma_support || dma_support > 2) {
-                dev_err(&host->dev,
-                        "Host Controller does not support IDMA Tx.\n");
-                host->dma_ops = NULL;
-                return -ENODEV;
-        }
-
-        dev_info(&host->dev, "Using internal DMA controller.\n");
-
         /* Forward link the descriptor list */
         for (i = 0, p = host->sg_cpu; i < host->ring_size - 1; i++, p++)
                 p->des3 = host->sg_dma + (sizeof(struct idmac_desc) * (i + 1));
@@ -476,7 +469,7 @@ static int dw_mci_pre_dma_transfer(struct dw_mci *host,
                         return -EINVAL;
         }
 
-        sg_len = dma_map_sg(&host->dev,
+        sg_len = dma_map_sg(host->dev,
                             data->sg,
                             data->sg_len,
                             dw_mci_get_dma_dir(data));
@@ -519,7 +512,7 @@ static void dw_mci_post_req(struct mmc_host *mmc,
                 return;
 
         if (data->host_cookie)
-                dma_unmap_sg(&slot->host->dev,
+                dma_unmap_sg(slot->host->dev,
                              data->sg,
                              data->sg_len,
                              dw_mci_get_dma_dir(data));
@@ -545,7 +538,7 @@ static int dw_mci_submit_data_dma(struct dw_mci *host, struct mmc_data *data)
 
         host->using_dma = 1;
 
-        dev_vdbg(&host->dev,
+        dev_vdbg(host->dev,
                  "sd sg_cpu: %#lx sg_dma: %#lx sg_len: %d\n",
                  (unsigned long)host->sg_cpu, (unsigned long)host->sg_dma,
                  sg_len);
@@ -814,6 +807,9 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
                 slot->clock = ios->clock;
         }
 
+        if (slot->host->drv_data->set_ios)
+                slot->host->drv_data->set_ios(slot->host, ios);
+
         switch (ios->power_mode) {
         case MMC_POWER_UP:
                 set_bit(DW_MMC_CARD_NEED_INIT, &slot->flags);
@@ -830,7 +826,9 @@ static int dw_mci_get_ro(struct mmc_host *mmc)
         struct dw_mci_board *brd = slot->host->pdata;
 
         /* Use platform get_ro function, else try on board write protect */
-        if (brd->get_ro)
+        if (brd->quirks & DW_MCI_QUIRK_NO_WRITE_PROTECT)
+                read_only = 0;
+        else if (brd->get_ro)
                 read_only = brd->get_ro(slot->id);
         else
                 read_only =
@@ -939,12 +937,12 @@ static void dw_mci_request_end(struct dw_mci *host, struct mmc_request *mrq)
                 slot = list_entry(host->queue.next,
                                   struct dw_mci_slot, queue_node);
                 list_del(&slot->queue_node);
-                dev_vdbg(&host->dev, "list not empty: %s is next\n",
+                dev_vdbg(host->dev, "list not empty: %s is next\n",
                          mmc_hostname(slot->mmc));
                 host->state = STATE_SENDING_CMD;
                 dw_mci_start_request(host, slot);
         } else {
-                dev_vdbg(&host->dev, "list empty\n");
+                dev_vdbg(host->dev, "list empty\n");
                 host->state = STATE_IDLE;
         }
 
@@ -1083,7 +1081,7 @@ static void dw_mci_tasklet_func(unsigned long priv)
                                         data->bytes_xfered = 0;
                                         data->error = -ETIMEDOUT;
                                 } else {
-                                        dev_err(&host->dev,
+                                        dev_err(host->dev,
                                                 "data FIFO error "
                                                 "(status=%08x)\n",
                                                 status);
@@ -1767,12 +1765,60 @@ static void dw_mci_work_routine_card(struct work_struct *work)
         }
 }
 
-static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
+#ifdef CONFIG_OF
+/* given a slot id, find out the device node representing that slot */
+static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
+{
+        struct device_node *np;
+        const __be32 *addr;
+        int len;
+
+        if (!dev || !dev->of_node)
+                return NULL;
+
+        for_each_child_of_node(dev->of_node, np) {
+                addr = of_get_property(np, "reg", &len);
+                if (!addr || (len < sizeof(int)))
+                        continue;
+                if (be32_to_cpup(addr) == slot)
+                        return np;
+        }
+        return NULL;
+}
+
+/* find out bus-width for a given slot */
+static u32 dw_mci_of_get_bus_wd(struct device *dev, u8 slot)
+{
+        struct device_node *np = dw_mci_of_find_slot_node(dev, slot);
+        u32 bus_wd = 1;
+
+        if (!np)
+                return 1;
+
+        if (of_property_read_u32(np, "bus-width", &bus_wd))
+                dev_err(dev, "bus-width property not found, assuming width"
+                               " as 1\n");
+        return bus_wd;
+}
+#else /* CONFIG_OF */
+static u32 dw_mci_of_get_bus_wd(struct device *dev, u8 slot)
+{
+        return 1;
+}
+static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
+{
+        return NULL;
+}
+#endif /* CONFIG_OF */
+
+static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 {
         struct mmc_host *mmc;
         struct dw_mci_slot *slot;
+        int ctrl_id, ret;
+        u8 bus_width;
 
-        mmc = mmc_alloc_host(sizeof(struct dw_mci_slot), &host->dev);
+        mmc = mmc_alloc_host(sizeof(struct dw_mci_slot), host->dev);
         if (!mmc)
                 return -ENOMEM;
 
@@ -1780,6 +1826,7 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
         slot->id = id;
         slot->mmc = mmc;
         slot->host = host;
+        host->slot[id] = slot;
 
         mmc->ops = &dw_mci_ops;
         mmc->f_min = DIV_ROUND_UP(host->bus_hz, 510);
@@ -1800,21 +1847,44 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
         if (host->pdata->caps)
                 mmc->caps = host->pdata->caps;
 
+        if (host->dev->of_node) {
+                ctrl_id = of_alias_get_id(host->dev->of_node, "mshc");
+                if (ctrl_id < 0)
+                        ctrl_id = 0;
+        } else {
+                ctrl_id = to_platform_device(host->dev)->id;
+        }
+        if (host->drv_data && host->drv_data->caps)
+                mmc->caps |= host->drv_data->caps[ctrl_id];
+
         if (host->pdata->caps2)
                 mmc->caps2 = host->pdata->caps2;
 
         if (host->pdata->get_bus_wd)
-                if (host->pdata->get_bus_wd(slot->id) >= 4)
-                        mmc->caps |= MMC_CAP_4_BIT_DATA;
+                bus_width = host->pdata->get_bus_wd(slot->id);
+        else if (host->dev->of_node)
+                bus_width = dw_mci_of_get_bus_wd(host->dev, slot->id);
+        else
+                bus_width = 1;
+
+        if (host->drv_data->setup_bus) {
+                struct device_node *slot_np;
+                slot_np = dw_mci_of_find_slot_node(host->dev, slot->id);
+                ret = host->drv_data->setup_bus(host, slot_np, bus_width);
+                if (ret)
+                        goto err_setup_bus;
+        }
+
+        switch (bus_width) {
+        case 8:
+                mmc->caps |= MMC_CAP_8_BIT_DATA;
+        case 4:
+                mmc->caps |= MMC_CAP_4_BIT_DATA;
+        }
 
         if (host->pdata->quirks & DW_MCI_QUIRK_HIGHSPEED)
                 mmc->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
 
-        if (mmc->caps2 & MMC_CAP2_POWEROFF_NOTIFY)
-                mmc->power_notify_type = MMC_HOST_PW_NOTIFY_SHORT;
-        else
-                mmc->power_notify_type = MMC_HOST_PW_NOTIFY_NONE;
-
         if (host->pdata->blk_settings) {
                 mmc->max_segs = host->pdata->blk_settings->max_segs;
                 mmc->max_blk_size = host->pdata->blk_settings->max_blk_size;
@@ -1850,7 +1920,6 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
         else
                 clear_bit(DW_MMC_CARD_PRESENT, &slot->flags);
 
-        host->slot[id] = slot;
         mmc_add_host(mmc);
 
 #if defined(CONFIG_DEBUG_FS)
@@ -1867,6 +1936,10 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
         queue_work(host->card_workqueue, &host->card_work);
 
         return 0;
+
+err_setup_bus:
+        mmc_free_host(mmc);
+        return -EINVAL;
 }
 
 static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
@@ -1884,10 +1957,10 @@ static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
 static void dw_mci_init_dma(struct dw_mci *host)
 {
         /* Alloc memory for sg translation */
-        host->sg_cpu = dma_alloc_coherent(&host->dev, PAGE_SIZE,
+        host->sg_cpu = dma_alloc_coherent(host->dev, PAGE_SIZE,
                                           &host->sg_dma, GFP_KERNEL);
         if (!host->sg_cpu) {
-                dev_err(&host->dev, "%s: could not alloc DMA memory\n",
+                dev_err(host->dev, "%s: could not alloc DMA memory\n",
                         __func__);
                 goto no_dma;
         }
@@ -1895,6 +1968,7 @@ static void dw_mci_init_dma(struct dw_mci *host)
         /* Determine which DMA interface to use */
 #ifdef CONFIG_MMC_DW_IDMAC
         host->dma_ops = &dw_mci_idmac_ops;
+        dev_info(&host->dev, "Using internal DMA controller.\n");
 #endif
 
         if (!host->dma_ops)
@@ -1903,12 +1977,12 @@ static void dw_mci_init_dma(struct dw_mci *host)
         if (host->dma_ops->init && host->dma_ops->start &&
             host->dma_ops->stop && host->dma_ops->cleanup) {
                 if (host->dma_ops->init(host)) {
-                        dev_err(&host->dev, "%s: Unable to initialize "
+                        dev_err(host->dev, "%s: Unable to initialize "
                                 "DMA Controller.\n", __func__);
                         goto no_dma;
                 }
         } else {
-                dev_err(&host->dev, "DMA initialization not found.\n");
+                dev_err(host->dev, "DMA initialization not found.\n");
                 goto no_dma;
         }
 
@@ -1916,7 +1990,7 @@ static void dw_mci_init_dma(struct dw_mci *host)
         return;
 
 no_dma:
-        dev_info(&host->dev, "Using PIO mode.\n");
+        dev_info(host->dev, "Using PIO mode.\n");
         host->use_dma = 0;
         return;
 }
@@ -1942,30 +2016,133 @@ static bool mci_wait_reset(struct device *dev, struct dw_mci *host)
         return false;
 }
 
+#ifdef CONFIG_OF
+static struct dw_mci_of_quirks {
+        char *quirk;
+        int id;
+} of_quirks[] = {
+        {
+                .quirk  = "supports-highspeed",
+                .id     = DW_MCI_QUIRK_HIGHSPEED,
+        }, {
+                .quirk  = "broken-cd",
+                .id     = DW_MCI_QUIRK_BROKEN_CARD_DETECTION,
+        },
+};
+
+static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
+{
+        struct dw_mci_board *pdata;
+        struct device *dev = host->dev;
+        struct device_node *np = dev->of_node;
+        int idx, ret;
+
+        pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+        if (!pdata) {
+                dev_err(dev, "could not allocate memory for pdata\n");
+                return ERR_PTR(-ENOMEM);
+        }
+
+        /* find out number of slots supported */
+        if (of_property_read_u32(dev->of_node, "num-slots",
+                                &pdata->num_slots)) {
+                dev_info(dev, "num-slots property not found, "
+                                "assuming 1 slot is available\n");
+                pdata->num_slots = 1;
+        }
+
+        /* get quirks */
+        for (idx = 0; idx < ARRAY_SIZE(of_quirks); idx++)
+                if (of_get_property(np, of_quirks[idx].quirk, NULL))
+                        pdata->quirks |= of_quirks[idx].id;
+
+        if (of_property_read_u32(np, "fifo-depth", &pdata->fifo_depth))
+                dev_info(dev, "fifo-depth property not found, using "
+                                "value of FIFOTH register as default\n");
+
+        of_property_read_u32(np, "card-detect-delay", &pdata->detect_delay_ms);
+
+        if (host->drv_data->parse_dt) {
+                ret = host->drv_data->parse_dt(host);
+                if (ret)
+                        return ERR_PTR(ret);
+        }
+
+        return pdata;
+}
+
+#else /* CONFIG_OF */
+static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
+{
+        return ERR_PTR(-EINVAL);
+}
+#endif /* CONFIG_OF */
+
 int dw_mci_probe(struct dw_mci *host)
 {
         int width, i, ret = 0;
         u32 fifo_size;
+        int init_slots = 0;
 
-        if (!host->pdata || !host->pdata->init) {
-                dev_err(&host->dev,
-                        "Platform data must supply init function\n");
-                return -ENODEV;
+        if (!host->pdata) {
+                host->pdata = dw_mci_parse_dt(host);
+                if (IS_ERR(host->pdata)) {
+                        dev_err(host->dev, "platform data not available\n");
+                        return -EINVAL;
+                }
         }
 
         if (!host->pdata->select_slot && host->pdata->num_slots > 1) {
-                dev_err(&host->dev,
+                dev_err(host->dev,
                         "Platform data must supply select_slot function\n");
                 return -ENODEV;
         }
 
-        if (!host->pdata->bus_hz) {
-                dev_err(&host->dev,
+        host->biu_clk = clk_get(host->dev, "biu");
+        if (IS_ERR(host->biu_clk)) {
+                dev_dbg(host->dev, "biu clock not available\n");
+        } else {
+                ret = clk_prepare_enable(host->biu_clk);
+                if (ret) {
+                        dev_err(host->dev, "failed to enable biu clock\n");
+                        clk_put(host->biu_clk);
+                        return ret;
+                }
+        }
+
+        host->ciu_clk = clk_get(host->dev, "ciu");
+        if (IS_ERR(host->ciu_clk)) {
+                dev_dbg(host->dev, "ciu clock not available\n");
+        } else {
+                ret = clk_prepare_enable(host->ciu_clk);
+                if (ret) {
+                        dev_err(host->dev, "failed to enable ciu clock\n");
+                        clk_put(host->ciu_clk);
+                        goto err_clk_biu;
+                }
+        }
+
+        if (IS_ERR(host->ciu_clk))
+                host->bus_hz = host->pdata->bus_hz;
+        else
+                host->bus_hz = clk_get_rate(host->ciu_clk);
+
+        if (host->drv_data->setup_clock) {
+                ret = host->drv_data->setup_clock(host);
+                if (ret) {
+                        dev_err(host->dev,
+                                "implementation specific clock setup failed\n");
+                        goto err_clk_ciu;
+                }
+        }
+
+        if (!host->bus_hz) {
+                dev_err(host->dev,
                         "Platform data must supply bus speed\n");
-                return -ENODEV;
+                ret = -ENODEV;
+                goto err_clk_ciu;
         }
 
-        host->bus_hz = host->pdata->bus_hz;
         host->quirks = host->pdata->quirks;
 
         spin_lock_init(&host->lock);
@@ -1998,7 +2175,7 @@ int dw_mci_probe(struct dw_mci *host)
         }
 
         /* Reset all blocks */
-        if (!mci_wait_reset(&host->dev, host))
+        if (!mci_wait_reset(host->dev, host))
                 return -ENODEV;
 
         host->dma_ops = host->pdata->dma_ops;
@@ -2054,10 +2231,18 @@ int dw_mci_probe(struct dw_mci *host)
         /* We need at least one slot to succeed */
         for (i = 0; i < host->num_slots; i++) {
                 ret = dw_mci_init_slot(host, i);
-                if (ret) {
-                        ret = -ENODEV;
-                        goto err_init_slot;
-                }
+                if (ret)
+                        dev_dbg(host->dev, "slot %d init failed\n", i);
+                else
+                        init_slots++;
+        }
+
+        if (init_slots) {
+                dev_info(host->dev, "%d slots initialized\n", init_slots);
+        } else {
+                dev_dbg(host->dev, "attempted to initialize %d slots, "
+                                        "but failed on all\n", host->num_slots);
+                goto err_init_slot;
         }
 
         /*
@@ -2065,7 +2250,7 @@ int dw_mci_probe(struct dw_mci *host)
          * Need to check the version-id and set data-offset for DATA register.
          */
         host->verid = SDMMC_GET_VERID(mci_readl(host, VERID));
-        dev_info(&host->dev, "Version ID is %04x\n", host->verid);
+        dev_info(host->dev, "Version ID is %04x\n", host->verid);
 
         if (host->verid < DW_MMC_240A)
                 host->data_offset = DATA_OFFSET;
@@ -2082,22 +2267,16 @@ int dw_mci_probe(struct dw_mci *host)
                    DW_MCI_ERROR_FLAGS | SDMMC_INT_CD);
         mci_writel(host, CTRL, SDMMC_CTRL_INT_ENABLE); /* Enable mci interrupt */
 
-        dev_info(&host->dev, "DW MMC controller at irq %d, "
+        dev_info(host->dev, "DW MMC controller at irq %d, "
                  "%d bit host data width, "
                  "%u deep fifo\n",
                  host->irq, width, fifo_size);
         if (host->quirks & DW_MCI_QUIRK_IDMAC_DTO)
-                dev_info(&host->dev, "Internal DMAC interrupt fix enabled.\n");
+                dev_info(host->dev, "Internal DMAC interrupt fix enabled.\n");
 
         return 0;
 
 err_init_slot:
-        /* De-init any initialized slots */
-        while (i > 0) {
-                if (host->slot[i])
-                        dw_mci_cleanup_slot(host->slot[i], i);
-                i--;
-        }
         free_irq(host->irq, host);
 
 err_workqueue:
@@ -2106,13 +2285,24 @@ err_workqueue:
 err_dmaunmap:
         if (host->use_dma && host->dma_ops->exit)
                 host->dma_ops->exit(host);
-        dma_free_coherent(&host->dev, PAGE_SIZE,
+        dma_free_coherent(host->dev, PAGE_SIZE,
                           host->sg_cpu, host->sg_dma);
 
         if (host->vmmc) {
                 regulator_disable(host->vmmc);
                 regulator_put(host->vmmc);
         }
+
+err_clk_ciu:
+        if (!IS_ERR(host->ciu_clk)) {
+                clk_disable_unprepare(host->ciu_clk);
+                clk_put(host->ciu_clk);
+        }
+err_clk_biu:
+        if (!IS_ERR(host->biu_clk)) {
+                clk_disable_unprepare(host->biu_clk);
+                clk_put(host->biu_clk);
+        }
         return ret;
 }
 EXPORT_SYMBOL(dw_mci_probe);
@@ -2125,7 +2315,7 @@ void dw_mci_remove(struct dw_mci *host)
         mci_writel(host, INTMASK, 0); /* disable all mmc interrupt first */
 
         for (i = 0; i < host->num_slots; i++) {
-                dev_dbg(&host->dev, "remove slot %d\n", i);
+                dev_dbg(host->dev, "remove slot %d\n", i);
                 if (host->slot[i])
                         dw_mci_cleanup_slot(host->slot[i], i);
         }
@@ -2136,7 +2326,7 @@ void dw_mci_remove(struct dw_mci *host)
 
         free_irq(host->irq, host);
         destroy_workqueue(host->card_workqueue);
-        dma_free_coherent(&host->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
+        dma_free_coherent(host->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
 
         if (host->use_dma && host->dma_ops->exit)
                 host->dma_ops->exit(host);
@@ -2146,6 +2336,12 @@ void dw_mci_remove(struct dw_mci *host)
                 regulator_put(host->vmmc);
         }
 
+        if (!IS_ERR(host->ciu_clk))
+                clk_disable_unprepare(host->ciu_clk);
+        if (!IS_ERR(host->biu_clk))
+                clk_disable_unprepare(host->biu_clk);
+        clk_put(host->ciu_clk);
+        clk_put(host->biu_clk);
 }
 EXPORT_SYMBOL(dw_mci_remove);
 
@@ -2188,7 +2384,7 @@ int dw_mci_resume(struct dw_mci *host)
         if (host->vmmc)
                 regulator_enable(host->vmmc);
 
-        if (!mci_wait_reset(&host->dev, host)) {
+        if (!mci_wait_reset(host->dev, host)) {
                 ret = -ENODEV;
                 return ret;
         }
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 15c27e17c23f..53b8fd987e47 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -182,4 +182,28 @@ extern int dw_mci_suspend(struct dw_mci *host);
 extern int dw_mci_resume(struct dw_mci *host);
 #endif
 
+/**
+ * dw_mci driver data - dw-mshc implementation specific driver data.
+ * @caps: mmc subsystem specified capabilities of the controller(s).
+ * @init: early implementation specific initialization.
+ * @setup_clock: implementation specific clock configuration.
+ * @prepare_command: handle CMD register extensions.
+ * @set_ios: handle bus specific extensions.
+ * @parse_dt: parse implementation specific device tree properties.
+ * @setup_bus: initialize io-interface
+ *
+ * Provide controller implementation specific extensions. The usage of this
+ * data structure is fully optional and usage of each member in this structure
+ * is optional as well.
+ */
+struct dw_mci_drv_data {
+        unsigned long   *caps;
+        int             (*init)(struct dw_mci *host);
+        int             (*setup_clock)(struct dw_mci *host);
+        void            (*prepare_command)(struct dw_mci *host, u32 *cmdr);
+        void            (*set_ios)(struct dw_mci *host, struct mmc_ios *ios);
+        int             (*parse_dt)(struct dw_mci *host);
+        int             (*setup_bus)(struct dw_mci *host,
+                                struct device_node *slot_np, u8 bus_width);
+};
 #endif /* _DW_MMC_H_ */
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index 273306c68d58..a600eabbd6c3 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -1532,20 +1532,7 @@ static struct spi_driver mmc_spi_driver = {
         .remove =       __devexit_p(mmc_spi_remove),
 };
 
-
-static int __init mmc_spi_init(void)
-{
-        return spi_register_driver(&mmc_spi_driver);
-}
-module_init(mmc_spi_init);
-
-
-static void __exit mmc_spi_exit(void)
-{
-        spi_unregister_driver(&mmc_spi_driver);
-}
-module_exit(mmc_spi_exit);
-
+module_spi_driver(mmc_spi_driver);
 
 MODULE_AUTHOR("Mike Lavender, David Brownell, "
                 "Hans-Peter Nilsson, Jan Nikitenko");
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 50ff19a62368..edc3e9baf0e7 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -1309,14 +1309,10 @@ static int __devinit mmci_probe(struct amba_device *dev,
                 goto host_free;
         }
 
-        ret = clk_prepare(host->clk);
+        ret = clk_prepare_enable(host->clk);
         if (ret)
                 goto clk_free;
 
-        ret = clk_enable(host->clk);
-        if (ret)
-                goto clk_unprep;
-
         host->plat = plat;
         host->variant = variant;
         host->mclk = clk_get_rate(host->clk);
@@ -1515,9 +1511,7 @@ static int __devinit mmci_probe(struct amba_device *dev,
  err_gpio_cd:
         iounmap(host->base);
  clk_disable:
-        clk_disable(host->clk);
- clk_unprep:
-        clk_unprepare(host->clk);
+        clk_disable_unprepare(host->clk);
  clk_free:
         clk_put(host->clk);
  host_free:
@@ -1564,8 +1558,7 @@ static int __devexit mmci_remove(struct amba_device *dev)
                         gpio_free(host->gpio_cd);
 
                 iounmap(host->base);
-                clk_disable(host->clk);
-                clk_unprepare(host->clk);
+                clk_disable_unprepare(host->clk);
                 clk_put(host->clk);
 
                 if (host->vcc)
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index 7b1161de01d6..565c2e4fac75 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -44,6 +44,7 @@
 #include <mach/hardware.h>
 
 #define DRIVER_NAME "mxc-mmc"
+#define MXCMCI_TIMEOUT_MS 10000
 
 #define MMC_REG_STR_STP_CLK             0x00
 #define MMC_REG_STATUS                  0x04
@@ -150,6 +151,8 @@ struct mxcmci_host {
         int                     dmareq;
         struct dma_slave_config dma_slave_config;
         struct imx_dma_data     dma_data;
+
+        struct timer_list       watchdog;
 };
 
 static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios);
@@ -271,9 +274,32 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
         dmaengine_submit(host->desc);
         dma_async_issue_pending(host->dma);
 
+        mod_timer(&host->watchdog, jiffies + msecs_to_jiffies(MXCMCI_TIMEOUT_MS));
+
         return 0;
 }
 
+static void mxcmci_cmd_done(struct mxcmci_host *host, unsigned int stat);
+static void mxcmci_data_done(struct mxcmci_host *host, unsigned int stat);
+
+static void mxcmci_dma_callback(void *data)
+{
+        struct mxcmci_host *host = data;
+        u32 stat;
+
+        del_timer(&host->watchdog);
+
+        stat = readl(host->base + MMC_REG_STATUS);
+        writel(stat & ~STATUS_DATA_TRANS_DONE, host->base + MMC_REG_STATUS);
+
+        dev_dbg(mmc_dev(host->mmc), "%s: 0x%08x\n", __func__, stat);
+
+        if (stat & STATUS_READ_OP_DONE)
+                writel(STATUS_READ_OP_DONE, host->base + MMC_REG_STATUS);
+
+        mxcmci_data_done(host, stat);
+}
+
 static int mxcmci_start_cmd(struct mxcmci_host *host, struct mmc_command *cmd,
                 unsigned int cmdat)
 {
@@ -305,8 +331,14 @@ static int mxcmci_start_cmd(struct mxcmci_host *host, struct mmc_command *cmd,
 
         int_cntr = INT_END_CMD_RES_EN;
 
-        if (mxcmci_use_dma(host))
-                int_cntr |= INT_READ_OP_EN | INT_WRITE_OP_DONE_EN;
+        if (mxcmci_use_dma(host)) {
+                if (host->dma_dir == DMA_FROM_DEVICE) {
+                        host->desc->callback = mxcmci_dma_callback;
+                        host->desc->callback_param = host;
+                } else {
+                        int_cntr |= INT_WRITE_OP_DONE_EN;
+                }
+        }
 
         spin_lock_irqsave(&host->lock, flags);
         if (host->use_sdio)
@@ -345,11 +377,9 @@ static int mxcmci_finish_data(struct mxcmci_host *host, unsigned int stat)
         struct mmc_data *data = host->data;
         int data_error;
 
-        if (mxcmci_use_dma(host)) {
-                dmaengine_terminate_all(host->dma);
+        if (mxcmci_use_dma(host))
                 dma_unmap_sg(host->dma->device->dev, data->sg, data->sg_len,
                                 host->dma_dir);
-        }
 
         if (stat & STATUS_ERR_MASK) {
                 dev_dbg(mmc_dev(host->mmc), "request failed. status: 0x%08x\n",
@@ -624,8 +654,10 @@ static irqreturn_t mxcmci_irq(int irq, void *devid)
                 mxcmci_cmd_done(host, stat);
 
         if (mxcmci_use_dma(host) &&
-                  (stat & (STATUS_DATA_TRANS_DONE | STATUS_WRITE_OP_DONE)))
+                  (stat & (STATUS_DATA_TRANS_DONE | STATUS_WRITE_OP_DONE))) {
+                del_timer(&host->watchdog);
                 mxcmci_data_done(host, stat);
+        }
 
         if (host->default_irq_mask &&
                   (stat & (STATUS_CARD_INSERTION | STATUS_CARD_REMOVAL)))
@@ -836,6 +868,34 @@ static bool filter(struct dma_chan *chan, void *param)
         return true;
 }
 
+static void mxcmci_watchdog(unsigned long data)
+{
+        struct mmc_host *mmc = (struct mmc_host *)data;
+        struct mxcmci_host *host = mmc_priv(mmc);
+        struct mmc_request *req = host->req;
+        unsigned int stat = readl(host->base + MMC_REG_STATUS);
+
+        if (host->dma_dir == DMA_FROM_DEVICE) {
+                dmaengine_terminate_all(host->dma);
+                dev_err(mmc_dev(host->mmc),
+                        "%s: read time out (status = 0x%08x)\n",
+                        __func__, stat);
+        } else {
+                dev_err(mmc_dev(host->mmc),
+                        "%s: write time out (status = 0x%08x)\n",
+                        __func__, stat);
+                mxcmci_softreset(host);
+        }
+
+        /* Mark transfer as erroneus and inform the upper layers */
+
+        host->data->error = -ETIMEDOUT;
+        host->req = NULL;
+        host->cmd = NULL;
+        host->data = NULL;
+        mmc_request_done(host->mmc, req);
+}
+
 static const struct mmc_host_ops mxcmci_ops = {
         .request                = mxcmci_request,
         .set_ios                = mxcmci_set_ios,
@@ -968,6 +1028,10 @@ static int mxcmci_probe(struct platform_device *pdev)
 
         mmc_add_host(mmc);
 
+        init_timer(&host->watchdog);
+        host->watchdog.function = &mxcmci_watchdog;
+        host->watchdog.data = (unsigned long)mmc;
+
         return 0;
 
 out_free_irq:
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index bb4c2bf04d09..80d1e6d4b0ae 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -525,7 +525,7 @@ static void mxs_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
                 writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
                        ssp->base + HW_SSP_CTRL0 + STMP_OFFSET_REG_SET);
                 writel(BM_SSP_CTRL1_SDIO_IRQ_EN,
-                       host->base + HW_SSP_CTRL1(host) + STMP_OFFSET_REG_SET);
+                       ssp->base + HW_SSP_CTRL1(ssp) + STMP_OFFSET_REG_SET);
         } else {
                 writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
                        ssp->base + HW_SSP_CTRL0 + STMP_OFFSET_REG_CLR);
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index c6259a829544..48ad361613ef 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -27,16 +27,10 @@
 #include <linux/mmc/card.h>
 #include <linux/clk.h>
 #include <linux/scatterlist.h>
-#include <linux/i2c/tps65010.h>
 #include <linux/slab.h>
 
-#include <asm/io.h>
-#include <asm/irq.h>
-
 #include <plat/mmc.h>
-#include <asm/gpio.h>
 #include <plat/dma.h>
-#include <plat/fpga.h>
 
 #define OMAP_MMC_REG_CMD        0x00
 #define OMAP_MMC_REG_ARGL       0x01
@@ -105,7 +99,6 @@ struct mmc_omap_slot {
         u16                     saved_con;
         u16                     bus_mode;
         unsigned int            fclk_freq;
-        unsigned                powered:1;
 
         struct tasklet_struct   cover_tasklet;
         struct timer_list       cover_timer;
@@ -137,7 +130,6 @@ struct mmc_omap_host {
         unsigned int            phys_base;
         int                     irq;
         unsigned char           bus_mode;
-        unsigned char           hw_bus_mode;
         unsigned int            reg_shift;
 
         struct work_struct      cmd_abort_work;
@@ -695,22 +687,29 @@ mmc_omap_xfer_data(struct mmc_omap_host *host, int write)
         host->buffer += nwords;
 }
 
-static inline void mmc_omap_report_irq(u16 status)
+#ifdef CONFIG_MMC_DEBUG
+static void mmc_omap_report_irq(struct mmc_omap_host *host, u16 status)
 {
         static const char *mmc_omap_status_bits[] = {
                 "EOC", "CD", "CB", "BRS", "EOFB", "DTO", "DCRC", "CTO",
                 "CCRC", "CRW", "AF", "AE", "OCRB", "CIRQ", "CERR"
         };
-        int i, c = 0;
+        int i;
+        char res[64], *buf = res;
+
+        buf += sprintf(buf, "MMC IRQ 0x%x:", status);
 
         for (i = 0; i < ARRAY_SIZE(mmc_omap_status_bits); i++)
-                if (status & (1 << i)) {
-                        if (c)
-                                printk(" ");
-                        printk("%s", mmc_omap_status_bits[i]);
-                        c++;
-                }
+                if (status & (1 << i))
+                        buf += sprintf(buf, " %s", mmc_omap_status_bits[i]);
+        dev_vdbg(mmc_dev(host->mmc), "%s\n", res);
 }
+#else
+static void mmc_omap_report_irq(struct mmc_omap_host *host, u16 status)
+{
+}
+#endif
+
 
 static irqreturn_t mmc_omap_irq(int irq, void *dev_id)
 {
@@ -744,12 +743,10 @@ static irqreturn_t mmc_omap_irq(int irq, void *dev_id)
                         cmd = host->cmd->opcode;
                 else
                         cmd = -1;
-#ifdef CONFIG_MMC_DEBUG
                 dev_dbg(mmc_dev(host->mmc), "MMC IRQ %04x (CMD %d): ",
                         status, cmd);
-                mmc_omap_report_irq(status);
-                printk("\n");
-#endif
+                mmc_omap_report_irq(host, status);
+
                 if (host->total_bytes_left) {
                         if ((status & OMAP_MMC_STAT_A_FULL) ||
                             (status & OMAP_MMC_STAT_END_OF_DATA))
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 38adc330c007..54bfd0cc106b 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -35,7 +35,6 @@
 #include <linux/mmc/core.h>
 #include <linux/mmc/mmc.h>
 #include <linux/io.h>
-#include <linux/semaphore.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
@@ -44,7 +43,6 @@
 #include <plat/cpu.h>
 
 /* OMAP HSMMC Host Controller Registers */
-#define OMAP_HSMMC_SYSCONFIG    0x0010
 #define OMAP_HSMMC_SYSSTATUS    0x0014
 #define OMAP_HSMMC_CON          0x002C
 #define OMAP_HSMMC_BLK          0x0104
@@ -161,8 +159,6 @@ struct omap_hsmmc_host {
         unsigned int            dma_sg_idx;
         unsigned char           bus_mode;
         unsigned char           power_mode;
-        u32                     *buffer;
-        u32                     bytesleft;
         int                     suspended;
         int                     irq;
         int                     use_dma, dma_ch;
@@ -171,7 +167,6 @@ struct omap_hsmmc_host {
         int                     slot_id;
         int                     response_busy;
         int                     context_loss;
-        int                     vdd;
         int                     protect_card;
         int                     reqs_blocked;
         int                     use_reg;
@@ -300,12 +295,12 @@ static int omap_hsmmc_reg_get(struct omap_hsmmc_host *host)
         struct regulator *reg;
         int ocr_value = 0;
 
-        mmc_slot(host).set_power = omap_hsmmc_set_power;
-
         reg = regulator_get(host->dev, "vmmc");
         if (IS_ERR(reg)) {
                 dev_dbg(host->dev, "vmmc regulator missing\n");
+                return PTR_ERR(reg);
         } else {
+                mmc_slot(host).set_power = omap_hsmmc_set_power;
                 host->vcc = reg;
                 ocr_value = mmc_regulator_get_ocrmask(reg);
                 if (!mmc_slot(host).ocr_mask) {
@@ -495,7 +490,7 @@ static void omap_hsmmc_set_clock(struct omap_hsmmc_host *host)
         unsigned long regval;
         unsigned long timeout;
 
-        dev_dbg(mmc_dev(host->mmc), "Set clock to %uHz\n", ios->clock);
+        dev_vdbg(mmc_dev(host->mmc), "Set clock to %uHz\n", ios->clock);
 
         omap_hsmmc_stop_clock(host);
 
@@ -579,21 +574,8 @@ static int omap_hsmmc_context_restore(struct omap_hsmmc_host *host)
         if (host->context_loss == context_loss)
                 return 1;
 
-        /* Wait for hardware reset */
-        timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
-        while ((OMAP_HSMMC_READ(host->base, SYSSTATUS) & RESETDONE) != RESETDONE
-                && time_before(jiffies, timeout))
-                ;
-
-        /* Do software reset */
-        OMAP_HSMMC_WRITE(host->base, SYSCONFIG, SOFTRESET);
-        timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
-        while ((OMAP_HSMMC_READ(host->base, SYSSTATUS) & RESETDONE) != RESETDONE
-                && time_before(jiffies, timeout))
-                ;
-
-        OMAP_HSMMC_WRITE(host->base, SYSCONFIG,
-                        OMAP_HSMMC_READ(host->base, SYSCONFIG) | AUTOIDLE);
+        if (!OMAP_HSMMC_READ(host->base, SYSSTATUS) & RESETDONE)
+                return 1;
 
         if (host->pdata->controller_flags & OMAP_HSMMC_SUPPORTS_DUAL_VOLT) {
                 if (host->power_mode != MMC_POWER_OFF &&
@@ -745,7 +727,7 @@ omap_hsmmc_start_command(struct omap_hsmmc_host *host, struct mmc_command *cmd,
 {
         int cmdreg = 0, resptype = 0, cmdtype = 0;
 
-        dev_dbg(mmc_dev(host->mmc), "%s: CMD%d, argument 0x%08x\n",
+        dev_vdbg(mmc_dev(host->mmc), "%s: CMD%d, argument 0x%08x\n",
                 mmc_hostname(host->mmc), cmd->opcode, cmd->arg);
         host->cmd = cmd;
 
@@ -934,7 +916,7 @@ static void omap_hsmmc_dbg_report_irq(struct omap_hsmmc_host *host, u32 status)
                         buf += len;
                 }
 
-        dev_dbg(mmc_dev(host->mmc), "%s\n", res);
+        dev_vdbg(mmc_dev(host->mmc), "%s\n", res);
 }
 #else
 static inline void omap_hsmmc_dbg_report_irq(struct omap_hsmmc_host *host,
@@ -981,72 +963,40 @@ static inline void omap_hsmmc_reset_controller_fsm(struct omap_hsmmc_host *host,
                         __func__);
 }
 
+static void hsmmc_command_incomplete(struct omap_hsmmc_host *host, int err)
+{
+        omap_hsmmc_reset_controller_fsm(host, SRC);
+        host->cmd->error = err;
+
+        if (host->data) {
+                omap_hsmmc_reset_controller_fsm(host, SRD);
+                omap_hsmmc_dma_cleanup(host, err);
+        }
+
+}
+
 static void omap_hsmmc_do_irq(struct omap_hsmmc_host *host, int status)
 {
         struct mmc_data *data;
         int end_cmd = 0, end_trans = 0;
 
-        if (!host->req_in_progress) {
-                do {
-                        OMAP_HSMMC_WRITE(host->base, STAT, status);
-                        /* Flush posted write */
-                        status = OMAP_HSMMC_READ(host->base, STAT);
-                } while (status & INT_EN_MASK);
-                return;
-        }
-
         data = host->data;
-        dev_dbg(mmc_dev(host->mmc), "IRQ Status is %x\n", status);
+        dev_vdbg(mmc_dev(host->mmc), "IRQ Status is %x\n", status);
 
         if (status & ERR) {
                 omap_hsmmc_dbg_report_irq(host, status);
-                if ((status & CMD_TIMEOUT) ||
-                        (status & CMD_CRC)) {
-                        if (host->cmd) {
-                                if (status & CMD_TIMEOUT) {
-                                        omap_hsmmc_reset_controller_fsm(host,
-                                                                        SRC);
-                                        host->cmd->error = -ETIMEDOUT;
-                                } else {
-                                        host->cmd->error = -EILSEQ;
-                                }
-                                end_cmd = 1;
-                        }
-                        if (host->data || host->response_busy) {
-                                if (host->data)
-                                        omap_hsmmc_dma_cleanup(host,
-                                                                -ETIMEDOUT);
-                                host->response_busy = 0;
-                                omap_hsmmc_reset_controller_fsm(host, SRD);
-                        }
-                }
-                if ((status & DATA_TIMEOUT) ||
-                        (status & DATA_CRC)) {
-                        if (host->data || host->response_busy) {
-                                int err = (status & DATA_TIMEOUT) ?
-                                                -ETIMEDOUT : -EILSEQ;
-
-                                if (host->data)
-                                        omap_hsmmc_dma_cleanup(host, err);
-                                else
-                                        host->mrq->cmd->error = err;
-                                host->response_busy = 0;
-                                omap_hsmmc_reset_controller_fsm(host, SRD);
-                                end_trans = 1;
-                        }
-                }
-                if (status & CARD_ERR) {
-                        dev_dbg(mmc_dev(host->mmc),
-                                "Ignoring card err CMD%d\n", host->cmd->opcode);
-                        if (host->cmd)
-                                end_cmd = 1;
-                        if (host->data)
-                                end_trans = 1;
+                if (status & (CMD_TIMEOUT | DATA_TIMEOUT))
+                        hsmmc_command_incomplete(host, -ETIMEDOUT);
+                else if (status & (CMD_CRC | DATA_CRC))
+                        hsmmc_command_incomplete(host, -EILSEQ);
+
+                end_cmd = 1;
+                if (host->data || host->response_busy) {
+                        end_trans = 1;
+                        host->response_busy = 0;
                 }
         }
 
-        OMAP_HSMMC_WRITE(host->base, STAT, status);
-
         if (end_cmd || ((status & CC) && host->cmd))
                 omap_hsmmc_cmd_done(host, host->cmd);
         if ((end_trans || (status & TC)) && host->mrq)
@@ -1062,11 +1012,13 @@ static irqreturn_t omap_hsmmc_irq(int irq, void *dev_id)
         int status;
 
         status = OMAP_HSMMC_READ(host->base, STAT);
-        do {
+        while (status & INT_EN_MASK && host->req_in_progress) {
                 omap_hsmmc_do_irq(host, status);
+
                 /* Flush posted write */
+                OMAP_HSMMC_WRITE(host->base, STAT, status);
                 status = OMAP_HSMMC_READ(host->base, STAT);
-        } while (status & INT_EN_MASK);
+        }
 
         return IRQ_HANDLED;
 }
@@ -1501,12 +1453,10 @@ static void omap_hsmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
                 case MMC_POWER_OFF:
                         mmc_slot(host).set_power(host->dev, host->slot_id,
                                                  0, 0);
-                        host->vdd = 0;
                         break;
                 case MMC_POWER_UP:
                         mmc_slot(host).set_power(host->dev, host->slot_id,
                                                  1, ios->vdd);
-                        host->vdd = ios->vdd;
                         break;
                 case MMC_POWER_ON:
                         do_send_init_stream = 1;
@@ -1598,10 +1548,6 @@ static void omap_hsmmc_conf_bus_power(struct omap_hsmmc_host *host)
         value = OMAP_HSMMC_READ(host->base, CAPA);
         OMAP_HSMMC_WRITE(host->base, CAPA, value | capa);
 
-        /* Set the controller to AUTO IDLE mode */
-        value = OMAP_HSMMC_READ(host->base, SYSCONFIG);
-        OMAP_HSMMC_WRITE(host->base, SYSCONFIG, value | AUTOIDLE);
-
         /* Set SD bus power bit */
         set_sd_bus_power(host);
 }
@@ -1659,8 +1605,6 @@ static int omap_hsmmc_regs_show(struct seq_file *s, void *data)
 
         pm_runtime_get_sync(host->dev);
 
-        seq_printf(s, "SYSCONFIG:\t0x%08x\n",
-                        OMAP_HSMMC_READ(host->base, SYSCONFIG));
         seq_printf(s, "CON:\t\t0x%08x\n",
                         OMAP_HSMMC_READ(host->base, CON));
         seq_printf(s, "HCTL:\t\t0x%08x\n",
@@ -2105,8 +2049,7 @@ static int omap_hsmmc_suspend(struct device *dev)
         if (ret) {
                 host->suspended = 0;
                 if (host->pdata->resume) {
-                        ret = host->pdata->resume(dev, host->slot_id);
-                        if (ret)
+                        if (host->pdata->resume(dev, host->slot_id))
                                 dev_dbg(dev, "Unmask interrupt failed\n");
                 }
                 goto err;
diff --git a/drivers/mmc/host/pxamci.c b/drivers/mmc/host/pxamci.c
index ca3915dac03d..3f9d6d577a91 100644
--- a/drivers/mmc/host/pxamci.c
+++ b/drivers/mmc/host/pxamci.c
@@ -30,6 +30,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/gpio.h>
 #include <linux/gfp.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/of_device.h>
 
 #include <asm/sizes.h>
 
@@ -573,6 +576,50 @@ static irqreturn_t pxamci_detect_irq(int irq, void *devid)
         return IRQ_HANDLED;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id pxa_mmc_dt_ids[] = {
+        { .compatible = "marvell,pxa-mmc" },
+        { }
+};
+
+MODULE_DEVICE_TABLE(of, pxa_mmc_dt_ids);
+
+static int __devinit pxamci_of_init(struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct pxamci_platform_data *pdata;
+        u32 tmp;
+
+        if (!np)
+                return 0;
+
+        pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+        if (!pdata)
+                return -ENOMEM;
+
+        pdata->gpio_card_detect =
+                of_get_named_gpio(np, "cd-gpios", 0);
+        pdata->gpio_card_ro =
+                of_get_named_gpio(np, "wp-gpios", 0);
+
+        /* pxa-mmc specific */
+        pdata->gpio_power =
+                of_get_named_gpio(np, "pxa-mmc,gpio-power", 0);
+
+        if (of_property_read_u32(np, "pxa-mmc,detect-delay-ms", &tmp) == 0)
+                pdata->detect_delay_ms = tmp;
+
+        pdev->dev.platform_data = pdata;
+
+        return 0;
+}
+#else
+static int __devinit pxamci_of_init(struct platform_device *pdev)
+{
+        return 0;
+}
+#endif
+
 static int pxamci_probe(struct platform_device *pdev)
 {
         struct mmc_host *mmc;
@@ -580,6 +627,10 @@ static int pxamci_probe(struct platform_device *pdev)
         struct resource *r, *dmarx, *dmatx;
         int ret, irq, gpio_cd = -1, gpio_ro = -1, gpio_power = -1;
 
+        ret = pxamci_of_init(pdev);
+        if (ret)
+                return ret;
+
         r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         irq = platform_get_irq(pdev, 0);
         if (!r || irq < 0)
@@ -866,6 +917,7 @@ static struct platform_driver pxamci_driver = {
         .driver         = {
                 .name   = DRIVER_NAME,
                 .owner  = THIS_MODULE,
+                .of_match_table = of_match_ptr(pxa_mmc_dt_ids),
 #ifdef CONFIG_PM
                 .pm     = &pxamci_pm_ops,
 #endif
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index a6e53a1ebb08..90140eb03e36 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -24,6 +24,7 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/mmc/host.h>
+#include <linux/of.h>
 
 #include "sdhci-pltfm.h"
 
@@ -126,11 +127,18 @@ static int __devexit sdhci_dove_remove(struct platform_device *pdev)
         return sdhci_pltfm_unregister(pdev);
 }
 
+static const struct of_device_id sdhci_dove_of_match_table[] __devinitdata = {
+        { .compatible = "marvell,dove-sdhci", },
+        {}
+};
+MODULE_DEVICE_TABLE(of, sdhci_dove_of_match_table);
+
 static struct platform_driver sdhci_dove_driver = {
         .driver         = {
                 .name   = "sdhci-dove",
                 .owner  = THIS_MODULE,
                 .pm     = SDHCI_PLTFM_PMOPS,
+                .of_match_table = of_match_ptr(sdhci_dove_of_match_table),
         },
         .probe          = sdhci_dove_probe,
         .remove         = __devexit_p(sdhci_dove_remove),
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index f8eb1fb0c921..ae5fcbfa1eef 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -21,6 +21,32 @@
 #include "sdhci-pltfm.h"
 #include "sdhci-esdhc.h"
 
+#define VENDOR_V_22     0x12
+static u32 esdhc_readl(struct sdhci_host *host, int reg)
+{
+        u32 ret;
+
+        ret = in_be32(host->ioaddr + reg);
+        /*
+         * The bit of ADMA flag in eSDHC is not compatible with standard
+         * SDHC register, so set fake flag SDHCI_CAN_DO_ADMA2 when ADMA is
+         * supported by eSDHC.
+         * And for many FSL eSDHC controller, the reset value of field
+         * SDHCI_CAN_DO_ADMA1 is one, but some of them can't support ADMA,
+         * only these vendor version is greater than 2.2/0x12 support ADMA.
+         * For FSL eSDHC, must aligned 4-byte, so use 0xFC to read the
+         * the verdor version number, oxFE is SDHCI_HOST_VERSION.
+         */
+        if ((reg == SDHCI_CAPABILITIES) && (ret & SDHCI_CAN_DO_ADMA1)) {
+                u32 tmp = in_be32(host->ioaddr + SDHCI_SLOT_INT_STATUS);
+                tmp = (tmp & SDHCI_VENDOR_VER_MASK) >> SDHCI_VENDOR_VER_SHIFT;
+                if (tmp > VENDOR_V_22)
+                        ret |= SDHCI_CAN_DO_ADMA2;
+        }
+
+        return ret;
+}
+
 static u16 esdhc_readw(struct sdhci_host *host, int reg)
 {
         u16 ret;
@@ -144,7 +170,7 @@ static void esdhc_of_resume(struct sdhci_host *host)
 #endif
 
 static struct sdhci_ops sdhci_esdhc_ops = {
-        .read_l = sdhci_be32bs_readl,
+        .read_l = esdhc_readl,
         .read_w = esdhc_readw,
         .read_b = esdhc_readb,
         .write_l = sdhci_be32bs_writel,
@@ -161,9 +187,13 @@ static struct sdhci_ops sdhci_esdhc_ops = {
 };
 
 static struct sdhci_pltfm_data sdhci_esdhc_pdata = {
-        /* card detection could be handled via GPIO */
+        /*
+         * card detection could be handled via GPIO
+         * eSDHC cannot support End Attribute in NOP ADMA descriptor
+         */
         .quirks = ESDHC_DEFAULT_QUIRKS | SDHCI_QUIRK_BROKEN_CARD_DETECTION
-                | SDHCI_QUIRK_NO_CARD_NO_RESET,
+                | SDHCI_QUIRK_NO_CARD_NO_RESET
+                | SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
         .ops = &sdhci_esdhc_ops,
 };
 
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 9722d43d6140..4bb74b042a06 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -1476,24 +1476,7 @@ static struct pci_driver sdhci_driver = {
         },
 };
 
-/*****************************************************************************\
- *                                                                           *
- * Driver init/exit                                                          *
- *                                                                           *
-\*****************************************************************************/
-
-static int __init sdhci_drv_init(void)
-{
-        return pci_register_driver(&sdhci_driver);
-}
-
-static void __exit sdhci_drv_exit(void)
-{
-        pci_unregister_driver(&sdhci_driver);
-}
-
-module_init(sdhci_drv_init);
-module_exit(sdhci_drv_exit);
+module_pci_driver(sdhci_driver);
 
 MODULE_AUTHOR("Pierre Ossman <pierre@ossman.eu>");
 MODULE_DESCRIPTION("Secure Digital Host Controller Interface PCI driver");
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index d9a4ef4f1ed0..65551a9709cc 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -75,6 +75,9 @@ void sdhci_get_of_property(struct platform_device *pdev)
                 if (sdhci_of_wp_inverted(np))
                         host->quirks |= SDHCI_QUIRK_INVERTED_WRITE_PROTECT;
 
+                if (of_get_property(np, "broken-cd", NULL))
+                        host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+
                 if (of_device_is_compatible(np, "fsl,p2020-rev1-esdhc"))
                         host->quirks |= SDHCI_QUIRK_BROKEN_DMA;
 
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index b6ee8857e226..8e63a9c04e31 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -197,7 +197,7 @@ static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
                 goto err_clk_get;
         }
         pltfm_host->clk = clk;
-        clk_enable(clk);
+        clk_prepare_enable(clk);
 
         host->quirks = SDHCI_QUIRK_BROKEN_ADMA
                 | SDHCI_QUIRK_BROKEN_TIMEOUT_VAL
@@ -239,7 +239,7 @@ static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
         return 0;
 
 err_add_host:
-        clk_disable(clk);
+        clk_disable_unprepare(clk);
         clk_put(clk);
 err_clk_get:
         sdhci_pltfm_free(pdev);
@@ -255,7 +255,7 @@ static int __devexit sdhci_pxav2_remove(struct platform_device *pdev)
 
         sdhci_remove_host(host, 1);
 
-        clk_disable(pltfm_host->clk);
+        clk_disable_unprepare(pltfm_host->clk);
         clk_put(pltfm_host->clk);
         sdhci_pltfm_free(pdev);
         kfree(pxa);
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index 07fe3834fe0b..e918a2bb3af1 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -24,12 +24,14 @@
 #include <linux/gpio.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/slot-gpio.h>
 #include <linux/platform_data/pxa_sdhci.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_gpio.h>
 
 #include "sdhci.h"
 #include "sdhci-pltfm.h"
@@ -182,6 +184,7 @@ static struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
         struct device_node *np = dev->of_node;
         u32 bus_width;
         u32 clk_delay_cycles;
+        enum of_gpio_flags gpio_flags;
 
         pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
         if (!pdata)
@@ -198,6 +201,10 @@ static struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
         if (clk_delay_cycles > 0)
                 pdata->clk_delay_cycles = clk_delay_cycles;
 
+        pdata->ext_cd_gpio = of_get_named_gpio_flags(np, "cd-gpios", 0, &gpio_flags);
+        if (gpio_flags != OF_GPIO_ACTIVE_LOW)
+                pdata->host_caps2 |= MMC_CAP2_CD_ACTIVE_HIGH;
+
         return pdata;
 }
 #else
@@ -231,14 +238,14 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
         pltfm_host = sdhci_priv(host);
         pltfm_host->priv = pxa;
 
-        clk = clk_get(dev, "PXA-SDHCLK");
+        clk = clk_get(dev, NULL);
         if (IS_ERR(clk)) {
                 dev_err(dev, "failed to get io clock\n");
                 ret = PTR_ERR(clk);
                 goto err_clk_get;
         }
         pltfm_host->clk = clk;
-        clk_enable(clk);
+        clk_prepare_enable(clk);
 
         host->quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL
                 | SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC
@@ -266,12 +273,25 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
                         host->quirks |= pdata->quirks;
                 if (pdata->host_caps)
                         host->mmc->caps |= pdata->host_caps;
+                if (pdata->host_caps2)
+                        host->mmc->caps2 |= pdata->host_caps2;
                 if (pdata->pm_caps)
                         host->mmc->pm_caps |= pdata->pm_caps;
+
+                if (gpio_is_valid(pdata->ext_cd_gpio)) {
+                        ret = mmc_gpio_request_cd(host->mmc, pdata->ext_cd_gpio);
+                        if (ret) {
+                                dev_err(mmc_dev(host->mmc),
+                                        "failed to allocate card detect gpio\n");
+                                goto err_cd_req;
+                        }
+                }
         }
 
         host->ops = &pxav3_sdhci_ops;
 
+        sdhci_get_of_property(pdev);
+
         ret = sdhci_add_host(host);
         if (ret) {
                 dev_err(&pdev->dev, "failed to add host\n");
@@ -283,8 +303,10 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
         return 0;
 
 err_add_host:
-        clk_disable(clk);
+        clk_disable_unprepare(clk);
         clk_put(clk);
+        mmc_gpio_free_cd(host->mmc);
+err_cd_req:
 err_clk_get:
         sdhci_pltfm_free(pdev);
         kfree(pxa);
@@ -296,11 +318,16 @@ static int __devexit sdhci_pxav3_remove(struct platform_device *pdev)
         struct sdhci_host *host = platform_get_drvdata(pdev);
         struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
         struct sdhci_pxa *pxa = pltfm_host->priv;
+        struct sdhci_pxa_platdata *pdata = pdev->dev.platform_data;
 
         sdhci_remove_host(host, 1);
 
-        clk_disable(pltfm_host->clk);
+        clk_disable_unprepare(pltfm_host->clk);
         clk_put(pltfm_host->clk);
+
+        if (gpio_is_valid(pdata->ext_cd_gpio))
+                mmc_gpio_free_cd(host->mmc);
+
         sdhci_pltfm_free(pdev);
         kfree(pxa);
 
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index a50c205ea208..2903949594c6 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -34,6 +34,9 @@
 
 #define MAX_BUS_CLK     (4)
 
+/* Number of gpio's used is max data bus width + command and clock lines */
+#define NUM_GPIOS(x)    (x + 2)
+
 /**
  * struct sdhci_s3c - S3C SDHCI instance
  * @host: The SDHCI host created
@@ -41,6 +44,7 @@
  * @ioarea: The resource created when we claimed the IO area.
  * @pdata: The platform data for this controller.
  * @cur_clk: The index of the current bus clock.
+ * @gpios: List of gpio numbers parsed from device tree.
  * @clk_io: The clock for the internal bus interface.
  * @clk_bus: The clocks that are available for the SD/MMC bus clock.
  */
@@ -52,6 +56,7 @@ struct sdhci_s3c {
         unsigned int            cur_clk;
         int                     ext_cd_irq;
         int                     ext_cd_gpio;
+        int                     *gpios;
 
         struct clk              *clk_io;
         struct clk              *clk_bus[MAX_BUS_CLK];
@@ -166,7 +171,7 @@ static unsigned int sdhci_s3c_consider_clock(struct sdhci_s3c *ourhost,
         dev_dbg(&ourhost->pdev->dev, "clk %d: rate %ld, want %d, got %ld\n",
                 src, rate, wanted, rate / div);
 
-        return (wanted - (rate / div));
+        return wanted - (rate / div);
 }
 
 /**
@@ -203,10 +208,12 @@ static void sdhci_s3c_set_clock(struct sdhci_host *host, unsigned int clock)
                  best_src, clock, best);
 
         /* select the new clock source */
-
         if (ourhost->cur_clk != best_src) {
                 struct clk *clk = ourhost->clk_bus[best_src];
 
+                clk_enable(clk);
+                clk_disable(ourhost->clk_bus[ourhost->cur_clk]);
+
                 /* turn clock off to card before changing clock source */
                 writew(0, host->ioaddr + SDHCI_CLOCK_CONTROL);
 
@@ -288,6 +295,7 @@ static unsigned int sdhci_cmu_get_min_clock(struct sdhci_host *host)
 static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
 {
         struct sdhci_s3c *ourhost = to_s3c(host);
+        struct device *dev = &ourhost->pdev->dev;
         unsigned long timeout;
         u16 clk = 0;
 
@@ -309,8 +317,8 @@ static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
         while (!((clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL))
                 & SDHCI_CLOCK_INT_STABLE)) {
                 if (timeout == 0) {
-                        printk(KERN_ERR "%s: Internal clock never "
-                                "stabilised.\n", mmc_hostname(host->mmc));
+                        dev_err(dev, "%s: Internal clock never stabilised.\n",
+                                mmc_hostname(host->mmc));
                         return;
                 }
                 timeout--;
@@ -404,7 +412,9 @@ static void sdhci_s3c_setup_card_detect_gpio(struct sdhci_s3c *sc)
                 if (sc->ext_cd_irq &&
                     request_threaded_irq(sc->ext_cd_irq, NULL,
                                          sdhci_s3c_gpio_card_detect_thread,
-                                         IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+                                         IRQF_TRIGGER_RISING |
+                                         IRQF_TRIGGER_FALLING |
+                                         IRQF_ONESHOT,
                                          dev_name(dev), sc) == 0) {
                         int status = gpio_get_value(sc->ext_cd_gpio);
                         if (pdata->ext_cd_gpio_invert)
@@ -419,9 +429,121 @@ static void sdhci_s3c_setup_card_detect_gpio(struct sdhci_s3c *sc)
         }
 }
 
+#ifdef CONFIG_OF
+static int __devinit sdhci_s3c_parse_dt(struct device *dev,
+                struct sdhci_host *host, struct s3c_sdhci_platdata *pdata)
+{
+        struct device_node *node = dev->of_node;
+        struct sdhci_s3c *ourhost = to_s3c(host);
+        u32 max_width;
+        int gpio, cnt, ret;
+
+        /* if the bus-width property is not specified, assume width as 1 */
+        if (of_property_read_u32(node, "bus-width", &max_width))
+                max_width = 1;
+        pdata->max_width = max_width;
+
+        ourhost->gpios = devm_kzalloc(dev, NUM_GPIOS(pdata->max_width) *
+                                sizeof(int), GFP_KERNEL);
+        if (!ourhost->gpios)
+                return -ENOMEM;
+
+        /* get the card detection method */
+        if (of_get_property(node, "broken-cd", 0)) {
+                pdata->cd_type = S3C_SDHCI_CD_NONE;
+                goto setup_bus;
+        }
+
+        if (of_get_property(node, "non-removable", 0)) {
+                pdata->cd_type = S3C_SDHCI_CD_PERMANENT;
+                goto setup_bus;
+        }
+
+        gpio = of_get_named_gpio(node, "cd-gpios", 0);
+        if (gpio_is_valid(gpio)) {
+                pdata->cd_type = S3C_SDHCI_CD_GPIO;
+                goto found_cd;
+        } else if (gpio != -ENOENT) {
+                dev_err(dev, "invalid card detect gpio specified\n");
+                return -EINVAL;
+        }
+
+        gpio = of_get_named_gpio(node, "samsung,cd-pinmux-gpio", 0);
+        if (gpio_is_valid(gpio)) {
+                pdata->cd_type = S3C_SDHCI_CD_INTERNAL;
+                goto found_cd;
+        } else if (gpio != -ENOENT) {
+                dev_err(dev, "invalid card detect gpio specified\n");
+                return -EINVAL;
+        }
+
+        dev_info(dev, "assuming no card detect line available\n");
+        pdata->cd_type = S3C_SDHCI_CD_NONE;
+
+ found_cd:
+        if (pdata->cd_type == S3C_SDHCI_CD_GPIO) {
+                pdata->ext_cd_gpio = gpio;
+                ourhost->ext_cd_gpio = -1;
+                if (of_get_property(node, "cd-inverted", NULL))
+                        pdata->ext_cd_gpio_invert = 1;
+        } else if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
+                ret = gpio_request(gpio, "sdhci-cd");
+                if (ret) {
+                        dev_err(dev, "card detect gpio request failed\n");
+                        return -EINVAL;
+                }
+                ourhost->ext_cd_gpio = gpio;
+        }
+
+ setup_bus:
+        /* get the gpios for command, clock and data lines */
+        for (cnt = 0; cnt < NUM_GPIOS(pdata->max_width); cnt++) {
+                gpio = of_get_gpio(node, cnt);
+                if (!gpio_is_valid(gpio)) {
+                        dev_err(dev, "invalid gpio[%d]\n", cnt);
+                        goto err_free_dt_cd_gpio;
+                }
+                ourhost->gpios[cnt] = gpio;
+        }
+
+        for (cnt = 0; cnt < NUM_GPIOS(pdata->max_width); cnt++) {
+                ret = gpio_request(ourhost->gpios[cnt], "sdhci-gpio");
+                if (ret) {
+                        dev_err(dev, "gpio[%d] request failed\n", cnt);
+                        goto err_free_dt_gpios;
+                }
+        }
+
+        return 0;
+
+ err_free_dt_gpios:
+        while (--cnt >= 0)
+                gpio_free(ourhost->gpios[cnt]);
+ err_free_dt_cd_gpio:
+        if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL)
+                gpio_free(ourhost->ext_cd_gpio);
+        return -EINVAL;
+}
+#else
+static int __devinit sdhci_s3c_parse_dt(struct device *dev,
+                struct sdhci_host *host, struct s3c_sdhci_platdata *pdata)
+{
+        return -EINVAL;
+}
+#endif
+
+static const struct of_device_id sdhci_s3c_dt_match[];
+
 static inline struct sdhci_s3c_drv_data *sdhci_s3c_get_driver_data(
                         struct platform_device *pdev)
 {
+#ifdef CONFIG_OF
+        if (pdev->dev.of_node) {
+                const struct of_device_id *match;
+                match = of_match_node(sdhci_s3c_dt_match, pdev->dev.of_node);
+                return (struct sdhci_s3c_drv_data *)match->data;
+        }
+#endif
         return (struct sdhci_s3c_drv_data *)
                         platform_get_device_id(pdev)->driver_data;
 }
@@ -436,7 +558,7 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
         struct resource *res;
         int ret, irq, ptr, clks;
 
-        if (!pdev->dev.platform_data) {
+        if (!pdev->dev.platform_data && !pdev->dev.of_node) {
                 dev_err(dev, "no device data specified\n");
                 return -ENOENT;
         }
@@ -452,21 +574,28 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
                 dev_err(dev, "sdhci_alloc_host() failed\n");
                 return PTR_ERR(host);
         }
+        sc = sdhci_priv(host);
 
         pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
         if (!pdata) {
                 ret = -ENOMEM;
-                goto err_io_clk;
+                goto err_pdata;
+        }
+
+        if (pdev->dev.of_node) {
+                ret = sdhci_s3c_parse_dt(&pdev->dev, host, pdata);
+                if (ret)
+                        goto err_pdata;
+        } else {
+                memcpy(pdata, pdev->dev.platform_data, sizeof(*pdata));
+                sc->ext_cd_gpio = -1; /* invalid gpio number */
         }
-        memcpy(pdata, pdev->dev.platform_data, sizeof(*pdata));
 
         drv_data = sdhci_s3c_get_driver_data(pdev);
-        sc = sdhci_priv(host);
 
         sc->host = host;
         sc->pdev = pdev;
         sc->pdata = pdata;
-        sc->ext_cd_gpio = -1; /* invalid gpio number */
 
         platform_set_drvdata(pdev, host);
 
@@ -486,9 +615,8 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 
                 snprintf(name, 14, "mmc_busclk.%d", ptr);
                 clk = clk_get(dev, name);
-                if (IS_ERR(clk)) {
+                if (IS_ERR(clk))
                         continue;
-                }
 
                 clks++;
                 sc->clk_bus[ptr] = clk;
@@ -499,8 +627,6 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
                  */
                 sc->cur_clk = ptr;
 
-                clk_enable(clk);
-
                 dev_info(dev, "clock source %d: %s (%ld Hz)\n",
                          ptr, name, clk_get_rate(clk));
         }
@@ -511,6 +637,10 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
                 goto err_no_busclks;
         }
 
+#ifndef CONFIG_PM_RUNTIME
+        clk_enable(sc->clk_bus[sc->cur_clk]);
+#endif
+
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         host->ioaddr = devm_request_and_ioremap(&pdev->dev, res);
         if (!host->ioaddr) {
@@ -616,12 +746,17 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
             gpio_is_valid(pdata->ext_cd_gpio))
                 sdhci_s3c_setup_card_detect_gpio(sc);
 
+#ifdef CONFIG_PM_RUNTIME
+        clk_disable(sc->clk_io);
+#endif
         return 0;
 
  err_req_regs:
+#ifndef CONFIG_PM_RUNTIME
+        clk_disable(sc->clk_bus[sc->cur_clk]);
+#endif
         for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
                 if (sc->clk_bus[ptr]) {
-                        clk_disable(sc->clk_bus[ptr]);
                         clk_put(sc->clk_bus[ptr]);
                 }
         }
@@ -631,6 +766,12 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
         clk_put(sc->clk_io);
 
  err_io_clk:
+        for (ptr = 0; ptr < NUM_GPIOS(sc->pdata->max_width); ptr++)
+                gpio_free(sc->gpios[ptr]);
+        if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL)
+                gpio_free(sc->ext_cd_gpio);
+
+ err_pdata:
         sdhci_free_host(host);
 
         return ret;
@@ -638,9 +779,9 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 
 static int __devexit sdhci_s3c_remove(struct platform_device *pdev)
 {
-        struct s3c_sdhci_platdata *pdata = pdev->dev.platform_data;
         struct sdhci_host *host =  platform_get_drvdata(pdev);
         struct sdhci_s3c *sc = sdhci_priv(host);
+        struct s3c_sdhci_platdata *pdata = sc->pdata;
         int ptr;
 
         if (pdata->cd_type == S3C_SDHCI_CD_EXTERNAL && pdata->ext_cd_cleanup)
@@ -652,19 +793,30 @@ static int __devexit sdhci_s3c_remove(struct platform_device *pdev)
         if (gpio_is_valid(sc->ext_cd_gpio))
                 gpio_free(sc->ext_cd_gpio);
 
+#ifdef CONFIG_PM_RUNTIME
+        clk_enable(sc->clk_io);
+#endif
         sdhci_remove_host(host, 1);
 
+        pm_runtime_dont_use_autosuspend(&pdev->dev);
         pm_runtime_disable(&pdev->dev);
 
-        for (ptr = 0; ptr < 3; ptr++) {
+#ifndef CONFIG_PM_RUNTIME
+        clk_disable(sc->clk_bus[sc->cur_clk]);
+#endif
+        for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
                 if (sc->clk_bus[ptr]) {
-                        clk_disable(sc->clk_bus[ptr]);
                         clk_put(sc->clk_bus[ptr]);
                 }
         }
         clk_disable(sc->clk_io);
         clk_put(sc->clk_io);
 
+        if (pdev->dev.of_node) {
+                for (ptr = 0; ptr < NUM_GPIOS(sc->pdata->max_width); ptr++)
+                        gpio_free(sc->gpios[ptr]);
+        }
+
         sdhci_free_host(host);
         platform_set_drvdata(pdev, NULL);
 
@@ -691,15 +843,28 @@ static int sdhci_s3c_resume(struct device *dev)
 static int sdhci_s3c_runtime_suspend(struct device *dev)
 {
         struct sdhci_host *host = dev_get_drvdata(dev);
+        struct sdhci_s3c *ourhost = to_s3c(host);
+        struct clk *busclk = ourhost->clk_io;
+        int ret;
+
+        ret = sdhci_runtime_suspend_host(host);
 
-        return sdhci_runtime_suspend_host(host);
+        clk_disable(ourhost->clk_bus[ourhost->cur_clk]);
+        clk_disable(busclk);
+        return ret;
 }
 
 static int sdhci_s3c_runtime_resume(struct device *dev)
 {
         struct sdhci_host *host = dev_get_drvdata(dev);
+        struct sdhci_s3c *ourhost = to_s3c(host);
+        struct clk *busclk = ourhost->clk_io;
+        int ret;
 
-        return sdhci_runtime_resume_host(host);
+        clk_enable(busclk);
+        clk_enable(ourhost->clk_bus[ourhost->cur_clk]);
+        ret = sdhci_runtime_resume_host(host);
+        return ret;
 }
 #endif
 
@@ -737,6 +902,16 @@ static struct platform_device_id sdhci_s3c_driver_ids[] = {
 };
 MODULE_DEVICE_TABLE(platform, sdhci_s3c_driver_ids);
 
+#ifdef CONFIG_OF
+static const struct of_device_id sdhci_s3c_dt_match[] = {
+        { .compatible = "samsung,s3c6410-sdhci", },
+        { .compatible = "samsung,exynos4210-sdhci",
+                .data = (void *)EXYNOS4_SDHCI_DRV_DATA },
+        {},
+};
+MODULE_DEVICE_TABLE(of, sdhci_s3c_dt_match);
+#endif
+
 static struct platform_driver sdhci_s3c_driver = {
         .probe          = sdhci_s3c_probe,
         .remove         = __devexit_p(sdhci_s3c_remove),
@@ -744,6 +919,7 @@ static struct platform_driver sdhci_s3c_driver = {
         .driver         = {
                 .owner  = THIS_MODULE,
                 .name   = "s3c-sdhci",
+                .of_match_table = of_match_ptr(sdhci_s3c_dt_match),
                 .pm     = SDHCI_S3C_PMOPS,
         },
 };
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index 423da8194cd8..6be89c032deb 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -20,6 +20,8 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/slab.h>
@@ -68,8 +70,42 @@ static irqreturn_t sdhci_gpio_irq(int irq, void *dev_id)
         return IRQ_HANDLED;
 }
 
+#ifdef CONFIG_OF
+static struct sdhci_plat_data * __devinit
+sdhci_probe_config_dt(struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct sdhci_plat_data *pdata = NULL;
+        int cd_gpio;
+
+        cd_gpio = of_get_named_gpio(np, "cd-gpios", 0);
+        if (!gpio_is_valid(cd_gpio))
+                cd_gpio = -1;
+
+        /* If pdata is required */
+        if (cd_gpio != -1) {
+                pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+                if (!pdata) {
+                        dev_err(&pdev->dev, "DT: kzalloc failed\n");
+                        return ERR_PTR(-ENOMEM);
+                }
+        }
+
+        pdata->card_int_gpio = cd_gpio;
+
+        return pdata;
+}
+#else
+static struct sdhci_plat_data * __devinit
+sdhci_probe_config_dt(struct platform_device *pdev)
+{
+        return ERR_PTR(-ENOSYS);
+}
+#endif
+
 static int __devinit sdhci_probe(struct platform_device *pdev)
 {
+        struct device_node *np = pdev->dev.of_node;
         struct sdhci_host *host;
         struct resource *iomem;
         struct spear_sdhci *sdhci;
@@ -104,14 +140,22 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
                 goto err;
         }
 
-        ret = clk_enable(sdhci->clk);
+        ret = clk_prepare_enable(sdhci->clk);
         if (ret) {
                 dev_dbg(&pdev->dev, "Error enabling clock\n");
                 goto put_clk;
         }
 
-        /* overwrite platform_data */
-        sdhci->data = dev_get_platdata(&pdev->dev);
+        if (np) {
+                sdhci->data = sdhci_probe_config_dt(pdev);
+                if (IS_ERR(sdhci->data)) {
+                        dev_err(&pdev->dev, "DT: Failed to get pdata\n");
+                        return -ENODEV;
+                }
+        } else {
+                sdhci->data = dev_get_platdata(&pdev->dev);
+        }
+
         pdev->dev.platform_data = sdhci;
 
         if (pdev->dev.parent)
@@ -216,7 +260,7 @@ set_drvdata:
 free_host:
         sdhci_free_host(host);
 disable_clk:
-        clk_disable(sdhci->clk);
+        clk_disable_unprepare(sdhci->clk);
 put_clk:
         clk_put(sdhci->clk);
 err:
@@ -238,7 +282,7 @@ static int __devexit sdhci_remove(struct platform_device *pdev)
 
         sdhci_remove_host(host, dead);
         sdhci_free_host(host);
-        clk_disable(sdhci->clk);
+        clk_disable_unprepare(sdhci->clk);
         clk_put(sdhci->clk);
 
         return 0;
@@ -253,7 +297,7 @@ static int sdhci_suspend(struct device *dev)
 
         ret = sdhci_suspend_host(host);
         if (!ret)
-                clk_disable(sdhci->clk);
+                clk_disable_unprepare(sdhci->clk);
 
         return ret;
 }
@@ -264,7 +308,7 @@ static int sdhci_resume(struct device *dev)
         struct spear_sdhci *sdhci = dev_get_platdata(dev);
         int ret;
 
-        ret = clk_enable(sdhci->clk);
+        ret = clk_prepare_enable(sdhci->clk);
         if (ret) {
                 dev_dbg(dev, "Resume: Error enabling clock\n");
                 return ret;
@@ -276,11 +320,20 @@ static int sdhci_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(sdhci_pm_ops, sdhci_suspend, sdhci_resume);
 
+#ifdef CONFIG_OF
+static const struct of_device_id sdhci_spear_id_table[] = {
+        { .compatible = "st,spear300-sdhci" },
+        {}
+};
+MODULE_DEVICE_TABLE(of, sdhci_spear_id_table);
+#endif
+
 static struct platform_driver sdhci_driver = {
         .driver = {
                 .name   = "sdhci",
                 .owner  = THIS_MODULE,
                 .pm     = &sdhci_pm_ops,
+                .of_match_table = of_match_ptr(sdhci_spear_id_table),
         },
         .probe          = sdhci_probe,
         .remove         = __devexit_p(sdhci_remove),
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index d43e7462941f..f9eb91623701 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -27,7 +27,6 @@
 
 #include <asm/gpio.h>
 
-#include <mach/gpio-tegra.h>
 #include <linux/platform_data/mmc-sdhci-tegra.h>
 
 #include "sdhci-pltfm.h"
@@ -257,10 +256,9 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
         int rc;
 
         match = of_match_device(sdhci_tegra_dt_match, &pdev->dev);
-        if (match)
-                soc_data = match->data;
-        else
-                soc_data = &soc_data_tegra20;
+        if (!match)
+                return -EINVAL;
+        soc_data = match->data;
 
         host = sdhci_pltfm_init(pdev, soc_data->pdata);
         if (IS_ERR(host))
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 9a11dc39921c..7922adb42386 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -28,6 +28,7 @@
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/slot-gpio.h>
 
 #include "sdhci.h"
 
@@ -1293,6 +1294,13 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
                 present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
                                 SDHCI_CARD_PRESENT;
 
+        /* If we're using a cd-gpio, testing the presence bit might fail. */
+        if (!present) {
+                int ret = mmc_gpio_get_cd(host->mmc);
+                if (ret > 0)
+                        present = true;
+        }
+
         if (!present || host->flags & SDHCI_DEVICE_DEAD) {
                 host->mrq->cmd->error = -ENOMEDIUM;
                 tasklet_schedule(&host->finish_tasklet);
@@ -1597,57 +1605,65 @@ static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
         spin_unlock_irqrestore(&host->lock, flags);
 }
 
-static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
-                                                struct mmc_ios *ios)
+static int sdhci_do_3_3v_signal_voltage_switch(struct sdhci_host *host,
+                                                u16 ctrl)
 {
-        u8 pwr;
-        u16 clk, ctrl;
-        u32 present_state;
+        int ret;
 
-        /*
-         * Signal Voltage Switching is only applicable for Host Controllers
-         * v3.00 and above.
-         */
-        if (host->version < SDHCI_SPEC_300)
-                return 0;
+        /* Set 1.8V Signal Enable in the Host Control2 register to 0 */
+        ctrl &= ~SDHCI_CTRL_VDD_180;
+        sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
 
-        /*
-         * We first check whether the request is to set signalling voltage
-         * to 3.3V. If so, we change the voltage to 3.3V and return quickly.
-         */
+        if (host->vqmmc) {
+                ret = regulator_set_voltage(host->vqmmc, 3300000, 3300000);
+                if (ret) {
+                        pr_warning("%s: Switching to 3.3V signalling voltage "
+                                   " failed\n", mmc_hostname(host->mmc));
+                        return -EIO;
+                }
+        }
+        /* Wait for 5ms */
+        usleep_range(5000, 5500);
+
+        /* 3.3V regulator output should be stable within 5 ms */
         ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-        if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
-                /* Set 1.8V Signal Enable in the Host Control2 register to 0 */
-                ctrl &= ~SDHCI_CTRL_VDD_180;
-                sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
+        if (!(ctrl & SDHCI_CTRL_VDD_180))
+                return 0;
 
-                /* Wait for 5ms */
-                usleep_range(5000, 5500);
+        pr_warning("%s: 3.3V regulator output did not became stable\n",
+                   mmc_hostname(host->mmc));
 
-                /* 3.3V regulator output should be stable within 5 ms */
-                ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-                if (!(ctrl & SDHCI_CTRL_VDD_180))
-                        return 0;
-                else {
-                        pr_info(DRIVER_NAME ": Switching to 3.3V "
-                                "signalling voltage failed\n");
-                        return -EIO;
-                }
-        } else if (!(ctrl & SDHCI_CTRL_VDD_180) &&
-                  (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180)) {
-                /* Stop SDCLK */
-                clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
-                clk &= ~SDHCI_CLOCK_CARD_EN;
-                sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+        return -EIO;
+}
 
-                /* Check whether DAT[3:0] is 0000 */
-                present_state = sdhci_readl(host, SDHCI_PRESENT_STATE);
-                if (!((present_state & SDHCI_DATA_LVL_MASK) >>
-                       SDHCI_DATA_LVL_SHIFT)) {
-                        /*
-                         * Enable 1.8V Signal Enable in the Host Control2
-                         * register
-                         */
+static int sdhci_do_1_8v_signal_voltage_switch(struct sdhci_host *host,
+                                                u16 ctrl)
+{
+        u8 pwr;
+        u16 clk;
+        u32 present_state;
+        int ret;
+
+        /* Stop SDCLK */
+        clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+        clk &= ~SDHCI_CLOCK_CARD_EN;
+        sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+
+        /* Check whether DAT[3:0] is 0000 */
+        present_state = sdhci_readl(host, SDHCI_PRESENT_STATE);
+        if (!((present_state & SDHCI_DATA_LVL_MASK) >>
+               SDHCI_DATA_LVL_SHIFT)) {
+                /*
+                 * Enable 1.8V Signal Enable in the Host Control2
+                 * register
+                 */
+                if (host->vqmmc)
+                        ret = regulator_set_voltage(host->vqmmc,
+                                1800000, 1800000);
+                else
+                        ret = 0;
+
+                if (!ret) {
                         ctrl |= SDHCI_CTRL_VDD_180;
                         sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
 
@@ -1656,7 +1672,7 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 
                         ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
                         if (ctrl & SDHCI_CTRL_VDD_180) {
-                                /* Provide SDCLK again and wait for 1ms*/
+                                /* Provide SDCLK again and wait for 1ms */
                                 clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
                                 clk |= SDHCI_CLOCK_CARD_EN;
                                 sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
@@ -1673,29 +1689,55 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
                                         return 0;
                         }
                 }
+        }
 
-                /*
-                 * If we are here, that means the switch to 1.8V signaling
-                 * failed. We power cycle the card, and retry initialization
-                 * sequence by setting S18R to 0.
-                 */
-                pwr = sdhci_readb(host, SDHCI_POWER_CONTROL);
-                pwr &= ~SDHCI_POWER_ON;
-                sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
-                if (host->vmmc)
-                        regulator_disable(host->vmmc);
+        /*
+         * If we are here, that means the switch to 1.8V signaling
+         * failed. We power cycle the card, and retry initialization
+         * sequence by setting S18R to 0.
+         */
+        pwr = sdhci_readb(host, SDHCI_POWER_CONTROL);
+        pwr &= ~SDHCI_POWER_ON;
+        sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+        if (host->vmmc)
+                regulator_disable(host->vmmc);
 
-                /* Wait for 1ms as per the spec */
-                usleep_range(1000, 1500);
-                pwr |= SDHCI_POWER_ON;
-                sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
-                if (host->vmmc)
-                        regulator_enable(host->vmmc);
+        /* Wait for 1ms as per the spec */
+        usleep_range(1000, 1500);
+        pwr |= SDHCI_POWER_ON;
+        sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+        if (host->vmmc)
+                regulator_enable(host->vmmc);
 
-                pr_info(DRIVER_NAME ": Switching to 1.8V signalling "
-                        "voltage failed, retrying with S18R set to 0\n");
-                return -EAGAIN;
-        } else
+        pr_warning("%s: Switching to 1.8V signalling voltage failed, "
+                   "retrying with S18R set to 0\n", mmc_hostname(host->mmc));
+
+        return -EAGAIN;
+}
+
+static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
+                                                struct mmc_ios *ios)
+{
+        u16 ctrl;
+
+        /*
+         * Signal Voltage Switching is only applicable for Host Controllers
+         * v3.00 and above.
+         */
+        if (host->version < SDHCI_SPEC_300)
+                return 0;
+
+        /*
+         * We first check whether the request is to set signalling voltage
+         * to 3.3V. If so, we change the voltage to 3.3V and return quickly.
+         */
+        ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+        if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330)
+                return sdhci_do_3_3v_signal_voltage_switch(host, ctrl);
+        else if (!(ctrl & SDHCI_CTRL_VDD_180) &&
+                        (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180))
+                return sdhci_do_1_8v_signal_voltage_switch(host, ctrl);
+        else
                 /* No signal voltage switch required */
                 return 0;
 }
@@ -2802,6 +2844,18 @@ int sdhci_add_host(struct sdhci_host *host)
             !(host->mmc->caps & MMC_CAP_NONREMOVABLE))
                 mmc->caps |= MMC_CAP_NEEDS_POLL;
 
+        /* If vqmmc regulator and no 1.8V signalling, then there's no UHS */
+        host->vqmmc = regulator_get(mmc_dev(mmc), "vqmmc");
+        if (IS_ERR(host->vqmmc)) {
+                pr_info("%s: no vqmmc regulator found\n", mmc_hostname(mmc));
+                host->vqmmc = NULL;
+        }
+        else if (regulator_is_supported_voltage(host->vqmmc, 1800000, 1800000))
+                regulator_enable(host->vqmmc);
+        else
+                caps[1] &= ~(SDHCI_SUPPORT_SDR104 | SDHCI_SUPPORT_SDR50 |
+                       SDHCI_SUPPORT_DDR50);
+
         /* Any UHS-I mode in caps implies SDR12 and SDR25 support. */
         if (caps[1] & (SDHCI_SUPPORT_SDR104 | SDHCI_SUPPORT_SDR50 |
                        SDHCI_SUPPORT_DDR50))
@@ -2832,15 +2886,6 @@ int sdhci_add_host(struct sdhci_host *host)
         if (caps[1] & SDHCI_DRIVER_TYPE_D)
                 mmc->caps |= MMC_CAP_DRIVER_TYPE_D;
 
-        /*
-         * If Power Off Notify capability is enabled by the host,
-         * set notify to short power off notify timeout value.
-         */
-        if (mmc->caps2 & MMC_CAP2_POWEROFF_NOTIFY)
-                mmc->power_notify_type = MMC_HOST_PW_NOTIFY_SHORT;
-        else
-                mmc->power_notify_type = MMC_HOST_PW_NOTIFY_NONE;
-
         /* Initial value for re-tuning timer count */
         host->tuning_count = (caps[1] & SDHCI_RETUNING_TIMER_COUNT_MASK) >>
                               SDHCI_RETUNING_TIMER_COUNT_SHIFT;
@@ -2862,7 +2907,8 @@ int sdhci_add_host(struct sdhci_host *host)
         if (IS_ERR(host->vmmc)) {
                 pr_info("%s: no vmmc regulator found\n", mmc_hostname(mmc));
                 host->vmmc = NULL;
-        }
+        } else
+                regulator_enable(host->vmmc);
 
 #ifdef CONFIG_REGULATOR
         if (host->vmmc) {
@@ -3119,8 +3165,15 @@ void sdhci_remove_host(struct sdhci_host *host, int dead)
         tasklet_kill(&host->card_tasklet);
         tasklet_kill(&host->finish_tasklet);
 
-        if (host->vmmc)
+        if (host->vmmc) {
+                regulator_disable(host->vmmc);
                 regulator_put(host->vmmc);
+        }
+
+        if (host->vqmmc) {
+                regulator_disable(host->vqmmc);
+                regulator_put(host->vqmmc);
+        }
 
         kfree(host->adma_desc);
         kfree(host->align_buffer);
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 5d8142773fac..11d2bc3b51d5 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -1213,7 +1213,9 @@ static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
                 sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_BUFRE);
                 sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MBUFRE);
         } else if (state & INT_DTRANE) {
-                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_DTRANE);
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT,
+                        ~(INT_CMD12DRE | INT_CMD12RBE |
+                          INT_CMD12CRE | INT_DTRANE));
                 sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MDTRANE);
         } else if (state & INT_CMD12RBE) {
                 sh_mmcif_writel(host->addr, MMCIF_CE_INT,
@@ -1229,6 +1231,10 @@ static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
                 host->sd_error = true;
                 dev_dbg(&host->pd->dev, "int err state = %08x\n", state);
         }
+        if (host->state == STATE_IDLE) {
+                dev_info(&host->pd->dev, "Spurious IRQ status 0x%x", state);
+                return IRQ_HANDLED;
+        }
         if (state & ~(INT_CMD12RBE | INT_CMD12CRE)) {
                 if (!host->dma_active)
                         return IRQ_WAKE_THREAD;
diff --git a/drivers/mmc/host/via-sdmmc.c b/drivers/mmc/host/via-sdmmc.c
index 4b83c43f950d..f18becef156d 100644
--- a/drivers/mmc/host/via-sdmmc.c
+++ b/drivers/mmc/host/via-sdmmc.c
@@ -1337,21 +1337,7 @@ static struct pci_driver via_sd_driver = {
         .resume = via_sd_resume,
 };
 
-static int __init via_sd_drv_init(void)
-{
-        pr_info(DRV_NAME ": VIA SD/MMC Card Reader driver "
-                "(C) 2008 VIA Technologies, Inc.\n");
-
-        return pci_register_driver(&via_sd_driver);
-}
-
-static void __exit via_sd_drv_exit(void)
-{
-        pci_unregister_driver(&via_sd_driver);
-}
-
-module_init(via_sd_drv_init);
-module_exit(via_sd_drv_exit);
+module_pci_driver(via_sd_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("VIA Technologies Inc.");
diff --git a/drivers/mmc/host/vub300.c b/drivers/mmc/host/vub300.c
index 58eab9ac1d01..d5655a63eda4 100644
--- a/drivers/mmc/host/vub300.c
+++ b/drivers/mmc/host/vub300.c
@@ -2358,9 +2358,9 @@ error5:
          * which is contained at the end of struct mmc
          */
 error4:
-        usb_free_urb(command_out_urb);
-error1:
         usb_free_urb(command_res_urb);
+error1:
+        usb_free_urb(command_out_urb);
 error0:
         return retval;
 }
diff --git a/drivers/mtd/devices/slram.c b/drivers/mtd/devices/slram.c
index 8f52fc858e48..5a5cd2ace4a6 100644
--- a/drivers/mtd/devices/slram.c
+++ b/drivers/mtd/devices/slram.c
@@ -240,7 +240,7 @@ static int parse_cmdline(char *devname, char *szstart, char *szlength)
 
         if (*(szlength) != '+') {
                 devlength = simple_strtoul(szlength, &buffer, 0);
-                devlength = handle_unit(devlength, buffer) - devstart;
+                devlength = handle_unit(devlength, buffer);
                 if (devlength < devstart)
                         goto err_out;
 
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index d5ece6ea6f98..1a03b7f673ce 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -2990,6 +2990,7 @@ static void nand_decode_ext_id(struct mtd_info *mtd, struct nand_chip *chip,
          * ID to decide what to do.
          */
         if (id_len == 6 && id_data[0] == NAND_MFR_SAMSUNG &&
+                        (chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
                         id_data[5] != 0x00) {
                 /* Calc pagesize */
                 mtd->writesize = 2048 << (extid & 0x03);
diff --git a/drivers/mtd/onenand/onenand_base.c b/drivers/mtd/onenand/onenand_base.c
index 7153e0d27101..b3f41f200622 100644
--- a/drivers/mtd/onenand/onenand_base.c
+++ b/drivers/mtd/onenand/onenand_base.c
@@ -3694,7 +3694,7 @@ static int flexonenand_check_blocks_erased(struct mtd_info *mtd, int start, int
  * flexonenand_set_boundary     - Writes the SLC boundary
  * @param mtd                   - mtd info structure
  */
-int flexonenand_set_boundary(struct mtd_info *mtd, int die,
+static int flexonenand_set_boundary(struct mtd_info *mtd, int die,
                                     int boundary, int lock)
 {
         struct onenand_chip *this = mtd->priv;
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index b721902bb6b4..b2530b002125 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -1519,7 +1519,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
         /* no need to lock since we're protected by rtnl_lock */
         if (slave_dev->features & NETIF_F_VLAN_CHALLENGED) {
                 pr_debug("%s: NETIF_F_VLAN_CHALLENGED\n", slave_dev->name);
-                if (bond_vlan_used(bond)) {
+                if (vlan_uses_dev(bond_dev)) {
                         pr_err("%s: Error: cannot enslave VLAN challenged slave %s on VLAN enabled bond %s\n",
                                bond_dev->name, slave_dev->name, bond_dev->name);
                         return -EPERM;
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index dc15d248443f..ef8d2a080d17 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -1060,7 +1060,7 @@ static ssize_t bonding_store_primary(struct device *d,
                 goto out;
         }
 
-        sscanf(buf, "%16s", ifname); /* IFNAMSIZ */
+        sscanf(buf, "%15s", ifname); /* IFNAMSIZ */
 
         /* check to see if we are clearing primary */
         if (!strlen(ifname) || buf[0] == '\n') {
@@ -1237,7 +1237,7 @@ static ssize_t bonding_store_active_slave(struct device *d,
                 goto out;
         }
 
-        sscanf(buf, "%16s", ifname); /* IFNAMSIZ */
+        sscanf(buf, "%15s", ifname); /* IFNAMSIZ */
 
         /* check to see if we are clearing active */
         if (!strlen(ifname) || buf[0] == '\n') {
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index c78ecfca1e45..a412bf6d73ef 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -144,9 +144,22 @@
 
 #define FLEXCAN_MB_CODE_MASK            (0xf0ffffff)
 
-/* FLEXCAN hardware feature flags */
+/*
+ * FLEXCAN hardware feature flags
+ *
+ * Below is some version info we got:
+ *    SOC   Version   IP-Version  Glitch-  [TR]WRN_INT
+ *                                Filter?   connected?
+ *   MX25  FlexCAN2  03.00.00.00     no         no
+ *   MX28  FlexCAN2  03.00.04.00    yes        yes
+ *   MX35  FlexCAN2  03.00.00.00     no         no
+ *   MX53  FlexCAN2  03.00.00.00    yes         no
+ *   MX6s  FlexCAN3  10.00.12.00    yes        yes
+ *
+ * Some SOCs do not have the RX_WARN & TX_WARN interrupt line connected.
+ */
 #define FLEXCAN_HAS_V10_FEATURES        BIT(1) /* For core version >= 10 */
-#define FLEXCAN_HAS_BROKEN_ERR_STATE    BIT(2) /* Broken error state handling */
+#define FLEXCAN_HAS_BROKEN_ERR_STATE    BIT(2) /* [TR]WRN_INT not connected */
 
 /* Structure of the message buffer */
 struct flexcan_mb {
@@ -205,7 +218,7 @@ static struct flexcan_devtype_data fsl_p1010_devtype_data = {
 };
 static struct flexcan_devtype_data fsl_imx28_devtype_data;
 static struct flexcan_devtype_data fsl_imx6q_devtype_data = {
-        .features = FLEXCAN_HAS_V10_FEATURES | FLEXCAN_HAS_BROKEN_ERR_STATE,
+        .features = FLEXCAN_HAS_V10_FEATURES,
 };
 
 static const struct can_bittiming_const flexcan_bittiming_const = {
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
index f5b82aeb2540..6525dbcca4e3 100644
--- a/drivers/net/can/sja1000/peak_pci.c
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -30,9 +30,10 @@
 
 #include "sja1000.h"
 
-MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
+MODULE_AUTHOR("Stephane Grosjean <s.grosjean@peak-system.com>");
 MODULE_DESCRIPTION("Socket-CAN driver for PEAK PCAN PCI family cards");
 MODULE_SUPPORTED_DEVICE("PEAK PCAN PCI/PCIe/PCIeC miniPCI CAN cards");
+MODULE_SUPPORTED_DEVICE("PEAK PCAN miniPCIe/cPCI PC/104+ PCI/104e CAN Cards");
 MODULE_LICENSE("GPL v2");
 
 #define DRV_NAME  "peak_pci"
@@ -64,7 +65,11 @@ struct peak_pci_chan {
 #define PEAK_PCI_DEVICE_ID      0x0001  /* for PCI/PCIe slot cards */
 #define PEAK_PCIEC_DEVICE_ID    0x0002  /* for ExpressCard slot cards */
 #define PEAK_PCIE_DEVICE_ID     0x0003  /* for nextgen PCIe slot cards */
-#define PEAK_MPCI_DEVICE_ID     0x0008  /* The miniPCI slot cards */
+#define PEAK_CPCI_DEVICE_ID     0x0004  /* for nextgen cPCI slot cards */
+#define PEAK_MPCI_DEVICE_ID     0x0005  /* for nextgen miniPCI slot cards */
+#define PEAK_PC_104P_DEVICE_ID  0x0006  /* PCAN-PC/104+ cards */
+#define PEAK_PCI_104E_DEVICE_ID 0x0007  /* PCAN-PCI/104 Express cards */
+#define PEAK_MPCIE_DEVICE_ID    0x0008  /* The miniPCIe slot cards */
 
 #define PEAK_PCI_CHAN_MAX       4
 
@@ -76,6 +81,10 @@ static DEFINE_PCI_DEVICE_TABLE(peak_pci_tbl) = {
         {PEAK_PCI_VENDOR_ID, PEAK_PCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
         {PEAK_PCI_VENDOR_ID, PEAK_PCIE_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
         {PEAK_PCI_VENDOR_ID, PEAK_MPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+        {PEAK_PCI_VENDOR_ID, PEAK_MPCIE_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+        {PEAK_PCI_VENDOR_ID, PEAK_PC_104P_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+        {PEAK_PCI_VENDOR_ID, PEAK_PCI_104E_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+        {PEAK_PCI_VENDOR_ID, PEAK_CPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
 #ifdef CONFIG_CAN_PEAK_PCIEC
         {PEAK_PCI_VENDOR_ID, PEAK_PCIEC_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
 #endif
diff --git a/drivers/net/ethernet/amd/amd8111e.c b/drivers/net/ethernet/amd/amd8111e.c
index 64d0d9c1afa2..3491d4312fc9 100644
--- a/drivers/net/ethernet/amd/amd8111e.c
+++ b/drivers/net/ethernet/amd/amd8111e.c
@@ -1845,6 +1845,7 @@ static int __devinit amd8111e_probe_one(struct pci_dev *pdev,
         if((pm_cap = pci_find_capability(pdev, PCI_CAP_ID_PM))==0){
                 printk(KERN_ERR "amd8111e: No Power Management capability, "
                        "exiting.\n");
+                err = -ENODEV;
                 goto err_free_reg;
         }
 
@@ -1852,6 +1853,7 @@ static int __devinit amd8111e_probe_one(struct pci_dev *pdev,
         if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32)) < 0) {
                 printk(KERN_ERR "amd8111e: DMA not supported,"
                         "exiting.\n");
+                err = -ENODEV;
                 goto err_free_reg;
         }
 
diff --git a/drivers/net/ethernet/amd/au1000_eth.c b/drivers/net/ethernet/amd/au1000_eth.c
index 397596b078d9..f195acfa2df7 100644
--- a/drivers/net/ethernet/amd/au1000_eth.c
+++ b/drivers/net/ethernet/amd/au1000_eth.c
@@ -1174,8 +1174,10 @@ static int __devinit au1000_probe(struct platform_device *pdev)
         snprintf(aup->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
                 pdev->name, aup->mac_id);
         aup->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
-        if (aup->mii_bus->irq == NULL)
+        if (aup->mii_bus->irq == NULL) {
+                err = -ENOMEM;
                 goto err_out;
+        }
 
         for (i = 0; i < PHY_MAX_ADDR; ++i)
                 aup->mii_bus->irq[i] = PHY_POLL;
@@ -1190,7 +1192,8 @@ static int __devinit au1000_probe(struct platform_device *pdev)
                 goto err_mdiobus_reg;
         }
 
-        if (au1000_mii_probe(dev) != 0)
+        err = au1000_mii_probe(dev);
+        if (err != 0)
                 goto err_out;
 
         pDBfree = NULL;
@@ -1205,6 +1208,7 @@ static int __devinit au1000_probe(struct platform_device *pdev)
         }
         aup->pDBfree = pDBfree;
 
+        err = -ENODEV;
         for (i = 0; i < NUM_RX_DMA; i++) {
                 pDB = au1000_GetFreeDB(aup);
                 if (!pDB)
@@ -1213,6 +1217,8 @@ static int __devinit au1000_probe(struct platform_device *pdev)
                 aup->rx_dma_ring[i]->buff_stat = (unsigned)pDB->dma_addr;
                 aup->rx_db_inuse[i] = pDB;
         }
+
+        err = -ENODEV;
         for (i = 0; i < NUM_TX_DMA; i++) {
                 pDB = au1000_GetFreeDB(aup);
                 if (!pDB)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 24220992413f..4833b6a9031c 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -2957,9 +2957,13 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
                         skb_shinfo(skb)->nr_frags +
                         BDS_PER_TX_PKT +
                         NEXT_CNT_PER_TX_PKT(MAX_BDS_PER_TX_PKT))) {
-                bnx2x_fp_qstats(bp, txdata->parent_fp)->driver_xoff++;
-                netif_tx_stop_queue(txq);
-                BNX2X_ERR("BUG! Tx ring full when queue awake!\n");
+                /* Handle special storage cases separately */
+                if (txdata->tx_ring_size != 0) {
+                        BNX2X_ERR("BUG! Tx ring full when queue awake!\n");
+                        bnx2x_fp_qstats(bp, txdata->parent_fp)->driver_xoff++;
+                        netif_tx_stop_queue(txq);
+                }
+
                 return NETDEV_TX_BUSY;
         }
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index e2e45ee5df33..6dd0dd076cc5 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -137,7 +137,16 @@
 #define LINK_20GTFD             LINK_STATUS_SPEED_AND_DUPLEX_20GTFD
 #define LINK_20GXFD             LINK_STATUS_SPEED_AND_DUPLEX_20GXFD
 
-
+#define LINK_UPDATE_MASK \
+                        (LINK_STATUS_SPEED_AND_DUPLEX_MASK | \
+                         LINK_STATUS_LINK_UP | \
+                         LINK_STATUS_PHYSICAL_LINK_FLAG | \
+                         LINK_STATUS_AUTO_NEGOTIATE_COMPLETE | \
+                         LINK_STATUS_RX_FLOW_CONTROL_FLAG_MASK | \
+                         LINK_STATUS_TX_FLOW_CONTROL_FLAG_MASK | \
+                         LINK_STATUS_PARALLEL_DETECTION_FLAG_MASK | \
+                         LINK_STATUS_LINK_PARTNER_SYMMETRIC_PAUSE | \
+                         LINK_STATUS_LINK_PARTNER_ASYMMETRIC_PAUSE)
 
 #define SFP_EEPROM_CON_TYPE_ADDR                0x2
         #define SFP_EEPROM_CON_TYPE_VAL_LC      0x7
@@ -3295,6 +3304,21 @@ static void bnx2x_serdes_deassert(struct bnx2x *bp, u8 port)
                DEFAULT_PHY_DEV_ADDR);
 }
 
+static void bnx2x_xgxs_specific_func(struct bnx2x_phy *phy,
+                                     struct link_params *params,
+                                     u32 action)
+{
+        struct bnx2x *bp = params->bp;
+        switch (action) {
+        case PHY_INIT:
+                /* Set correct devad */
+                REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_ST + params->port*0x18, 0);
+                REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + params->port*0x18,
+                       phy->def_md_devad);
+                break;
+        }
+}
+
 static void bnx2x_xgxs_deassert(struct link_params *params)
 {
         struct bnx2x *bp = params->bp;
@@ -3309,10 +3333,8 @@ static void bnx2x_xgxs_deassert(struct link_params *params)
         REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_CLEAR, val);
         udelay(500);
         REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_SET, val);
-
-        REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_ST + port*0x18, 0);
-        REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + port*0x18,
-               params->phy[INT_PHY].def_md_devad);
+        bnx2x_xgxs_specific_func(&params->phy[INT_PHY], params,
+                                 PHY_INIT);
 }
 
 static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
@@ -3545,14 +3567,11 @@ static void bnx2x_warpcore_set_lpi_passthrough(struct bnx2x_phy *phy,
 static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
                                         struct link_params *params,
                                         struct link_vars *vars) {
-        u16 val16 = 0, lane, i;
+        u16 lane, i, cl72_ctrl, an_adv = 0;
+        u16 ucode_ver;
         struct bnx2x *bp = params->bp;
         static struct bnx2x_reg_set reg_set[] = {
                 {MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x7},
-                {MDIO_AN_DEVAD, MDIO_WC_REG_PAR_DET_10G_CTRL, 0},
-                {MDIO_WC_DEVAD, MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL, 0},
-                {MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL0, 0xff},
-                {MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL1, 0x5555},
                 {MDIO_PMA_DEVAD, MDIO_WC_REG_IEEE0BLK_AUTONEGNP, 0x0},
                 {MDIO_WC_DEVAD, MDIO_WC_REG_RX66_CONTROL, 0x7415},
                 {MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_MISC2, 0x6190},
@@ -3565,12 +3584,19 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
                 bnx2x_cl45_write(bp, phy, reg_set[i].devad, reg_set[i].reg,
                                  reg_set[i].val);
 
+        bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
+                MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL, &cl72_ctrl);
+        cl72_ctrl &= 0xf8ff;
+        cl72_ctrl |= 0x3800;
+        bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
+                MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL, cl72_ctrl);
+
         /* Check adding advertisement for 1G KX */
         if (((vars->line_speed == SPEED_AUTO_NEG) &&
              (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_1G)) ||
             (vars->line_speed == SPEED_1000)) {
                 u32 addr = MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2;
-                val16 |= (1<<5);
+                an_adv |= (1<<5);
 
                 /* Enable CL37 1G Parallel Detect */
                 bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD, addr, 0x1);
@@ -3580,11 +3606,14 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
              (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)) ||
             (vars->line_speed ==  SPEED_10000)) {
                 /* Check adding advertisement for 10G KR */
-                val16 |= (1<<7);
+                an_adv |= (1<<7);
                 /* Enable 10G Parallel Detect */
+                CL22_WR_OVER_CL45(bp, phy, MDIO_REG_BANK_AER_BLOCK,
+                                  MDIO_AER_BLOCK_AER_REG, 0);
+
                 bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
                                  MDIO_WC_REG_PAR_DET_10G_CTRL, 1);
-
+                bnx2x_set_aer_mmd(params, phy);
                 DP(NETIF_MSG_LINK, "Advertize 10G\n");
         }
 
@@ -3604,7 +3633,7 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 
         /* Advertised speeds */
         bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-                         MDIO_WC_REG_AN_IEEE1BLK_AN_ADVERTISEMENT1, val16);
+                         MDIO_WC_REG_AN_IEEE1BLK_AN_ADVERTISEMENT1, an_adv);
 
         /* Advertised and set FEC (Forward Error Correction) */
         bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
@@ -3628,9 +3657,10 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
         /* Set KR Autoneg Work-Around flag for Warpcore version older than D108
          */
         bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-                        MDIO_WC_REG_UC_INFO_B1_VERSION, &val16);
-        if (val16 < 0xd108) {
-                DP(NETIF_MSG_LINK, "Enable AN KR work-around\n");
+                        MDIO_WC_REG_UC_INFO_B1_VERSION, &ucode_ver);
+        if (ucode_ver < 0xd108) {
+                DP(NETIF_MSG_LINK, "Enable AN KR work-around. WC ver:0x%x\n",
+                               ucode_ver);
                 vars->rx_tx_asic_rst = MAX_KR_LINK_RETRY;
         }
         bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
@@ -3651,21 +3681,16 @@ static void bnx2x_warpcore_set_10G_KR(struct bnx2x_phy *phy,
                                       struct link_vars *vars)
 {
         struct bnx2x *bp = params->bp;
-        u16 i;
+        u16 val16, i, lane;
         static struct bnx2x_reg_set reg_set[] = {
                 /* Disable Autoneg */
                 {MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x7},
-                {MDIO_AN_DEVAD, MDIO_WC_REG_PAR_DET_10G_CTRL, 0},
                 {MDIO_WC_DEVAD, MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL,
                         0x3f00},
                 {MDIO_AN_DEVAD, MDIO_WC_REG_AN_IEEE1BLK_AN_ADVERTISEMENT1, 0},
                 {MDIO_AN_DEVAD, MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x0},
                 {MDIO_WC_DEVAD, MDIO_WC_REG_DIGITAL3_UP1, 0x1},
                 {MDIO_WC_DEVAD, MDIO_WC_REG_DIGITAL5_MISC7, 0xa},
-                /* Disable CL36 PCS Tx */
-                {MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL0, 0x0},
-                /* Double Wide Single Data Rate @ pll rate */
-                {MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL1, 0xFFFF},
                 /* Leave cl72 training enable, needed for KR */
                 {MDIO_PMA_DEVAD,
                 MDIO_WC_REG_PMD_IEEE9BLK_TENGBASE_KR_PMD_CONTROL_REGISTER_150,
@@ -3676,11 +3701,24 @@ static void bnx2x_warpcore_set_10G_KR(struct bnx2x_phy *phy,
                 bnx2x_cl45_write(bp, phy, reg_set[i].devad, reg_set[i].reg,
                                  reg_set[i].val);
 
-        /* Leave CL72 enabled */
-        bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
-                                 MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL,
-                                 0x3800);
+        lane = bnx2x_get_warpcore_lane(phy, params);
+        /* Global registers */
+        CL22_WR_OVER_CL45(bp, phy, MDIO_REG_BANK_AER_BLOCK,
+                          MDIO_AER_BLOCK_AER_REG, 0);
+        /* Disable CL36 PCS Tx */
+        bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
+                        MDIO_WC_REG_XGXSBLK1_LANECTRL0, &val16);
+        val16 &= ~(0x0011 << lane);
+        bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
+                         MDIO_WC_REG_XGXSBLK1_LANECTRL0, val16);
 
+        bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
+                        MDIO_WC_REG_XGXSBLK1_LANECTRL1, &val16);
+        val16 |= (0x0303 << (lane << 1));
+        bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
+                         MDIO_WC_REG_XGXSBLK1_LANECTRL1, val16);
+        /* Restore AER */
+        bnx2x_set_aer_mmd(params, phy);
         /* Set speed via PMA/PMD register */
         bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD,
                          MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x2040);
@@ -4303,7 +4341,7 @@ static void bnx2x_warpcore_link_reset(struct bnx2x_phy *phy,
                                       struct link_params *params)
 {
         struct bnx2x *bp = params->bp;
-        u16 val16;
+        u16 val16, lane;
         bnx2x_sfp_e3_set_transmitter(params, phy, 0);
         bnx2x_set_mdio_clk(bp, params->chip_id, params->port);
         bnx2x_set_aer_mmd(params, phy);
@@ -4340,6 +4378,30 @@ static void bnx2x_warpcore_link_reset(struct bnx2x_phy *phy,
                          MDIO_WC_REG_XGXSBLK1_LANECTRL2,
                          val16 & 0xff00);
 
+        lane = bnx2x_get_warpcore_lane(phy, params);
+        /* Disable CL36 PCS Tx */
+        bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
+                        MDIO_WC_REG_XGXSBLK1_LANECTRL0, &val16);
+        val16 |= (0x11 << lane);
+        if (phy->flags & FLAGS_WC_DUAL_MODE)
+                val16 |= (0x22 << lane);
+        bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
+                         MDIO_WC_REG_XGXSBLK1_LANECTRL0, val16);
+
+        bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
+                        MDIO_WC_REG_XGXSBLK1_LANECTRL1, &val16);
+        val16 &= ~(0x0303 << (lane << 1));
+        val16 |= (0x0101 << (lane << 1));
+        if (phy->flags & FLAGS_WC_DUAL_MODE) {
+                val16 &= ~(0x0c0c << (lane << 1));
+                val16 |= (0x0404 << (lane << 1));
+        }
+
+        bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
+                         MDIO_WC_REG_XGXSBLK1_LANECTRL1, val16);
+        /* Restore AER */
+        bnx2x_set_aer_mmd(params, phy);
+
 }
 
 static void bnx2x_set_warpcore_loopback(struct bnx2x_phy *phy,
@@ -6296,15 +6358,7 @@ static int bnx2x_update_link_down(struct link_params *params,
         vars->mac_type = MAC_TYPE_NONE;
 
         /* Update shared memory */
-        vars->link_status &= ~(LINK_STATUS_SPEED_AND_DUPLEX_MASK |
-                               LINK_STATUS_LINK_UP |
-                               LINK_STATUS_PHYSICAL_LINK_FLAG |
-                               LINK_STATUS_AUTO_NEGOTIATE_COMPLETE |
-                               LINK_STATUS_RX_FLOW_CONTROL_FLAG_MASK |
-                               LINK_STATUS_TX_FLOW_CONTROL_FLAG_MASK |
-                               LINK_STATUS_PARALLEL_DETECTION_FLAG_MASK |
-                               LINK_STATUS_LINK_PARTNER_SYMMETRIC_PAUSE |
-                               LINK_STATUS_LINK_PARTNER_ASYMMETRIC_PAUSE);
+        vars->link_status &= ~LINK_UPDATE_MASK;
         vars->line_speed = 0;
         bnx2x_update_mng(params, vars->link_status);
 
@@ -6452,6 +6506,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
         u16 ext_phy_line_speed = 0, prev_line_speed = vars->line_speed;
         u8 active_external_phy = INT_PHY;
         vars->phy_flags &= ~PHY_HALF_OPEN_CONN_FLAG;
+        vars->link_status &= ~LINK_UPDATE_MASK;
         for (phy_index = INT_PHY; phy_index < params->num_phys;
               phy_index++) {
                 phy_vars[phy_index].flow_ctrl = 0;
@@ -7579,7 +7634,7 @@ static void bnx2x_warpcore_power_module(struct link_params *params,
 static int bnx2x_warpcore_read_sfp_module_eeprom(struct bnx2x_phy *phy,
                                                  struct link_params *params,
                                                  u16 addr, u8 byte_cnt,
-                                                 u8 *o_buf)
+                                                 u8 *o_buf, u8 is_init)
 {
         int rc = 0;
         u8 i, j = 0, cnt = 0;
@@ -7596,10 +7651,10 @@ static int bnx2x_warpcore_read_sfp_module_eeprom(struct bnx2x_phy *phy,
         /* 4 byte aligned address */
         addr32 = addr & (~0x3);
         do {
-                if (cnt == I2C_WA_PWR_ITER) {
+                if ((!is_init) && (cnt == I2C_WA_PWR_ITER)) {
                         bnx2x_warpcore_power_module(params, phy, 0);
                         /* Note that 100us are not enough here */
-                        usleep_range(1000,1000);
+                        usleep_range(1000, 2000);
                         bnx2x_warpcore_power_module(params, phy, 1);
                 }
                 rc = bnx2x_bsc_read(params, phy, 0xa0, addr32, 0, byte_cnt,
@@ -7719,7 +7774,7 @@ int bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
         break;
         case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT:
                 rc = bnx2x_warpcore_read_sfp_module_eeprom(phy, params, addr,
-                                                           byte_cnt, o_buf);
+                                                           byte_cnt, o_buf, 0);
         break;
         }
         return rc;
@@ -7923,6 +7978,7 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
 
 {
         u8 val;
+        int rc;
         struct bnx2x *bp = params->bp;
         u16 timeout;
         /* Initialization time after hot-plug may take up to 300ms for
@@ -7930,8 +7986,14 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
          */
 
         for (timeout = 0; timeout < 60; timeout++) {
-                if (bnx2x_read_sfp_module_eeprom(phy, params, 1, 1, &val)
-                    == 0) {
+                if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT)
+                        rc = bnx2x_warpcore_read_sfp_module_eeprom(phy,
+                                                                   params, 1,
+                                                                   1, &val, 1);
+                else
+                        rc = bnx2x_read_sfp_module_eeprom(phy, params, 1, 1,
+                                                          &val);
+                if (rc == 0) {
                         DP(NETIF_MSG_LINK,
                            "SFP+ module initialization took %d ms\n",
                            timeout * 5);
@@ -7939,7 +8001,8 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
                 }
                 usleep_range(5000, 10000);
         }
-        return -EINVAL;
+        rc = bnx2x_read_sfp_module_eeprom(phy, params, 1, 1, &val);
+        return rc;
 }
 
 static void bnx2x_8727_power_module(struct bnx2x *bp,
@@ -10993,7 +11056,7 @@ static struct bnx2x_phy phy_xgxs = {
         .format_fw_ver  = (format_fw_ver_t)NULL,
         .hw_reset       = (hw_reset_t)NULL,
         .set_link_led   = (set_link_led_t)NULL,
-        .phy_specific_func = (phy_specific_func_t)NULL
+        .phy_specific_func = (phy_specific_func_t)bnx2x_xgxs_specific_func
 };
 static struct bnx2x_phy phy_warpcore = {
         .type           = PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT,
@@ -11465,6 +11528,11 @@ static int bnx2x_populate_int_phy(struct bnx2x *bp, u32 shmem_base, u8 port,
                         phy->media_type = ETH_PHY_BASE_T;
                         break;
                 case PORT_HW_CFG_NET_SERDES_IF_XFI:
+                        phy->supported &= (SUPPORTED_1000baseT_Full |
+                                           SUPPORTED_10000baseT_Full |
+                                           SUPPORTED_FIBRE |
+                                           SUPPORTED_Pause |
+                                           SUPPORTED_Asym_Pause);
                         phy->media_type = ETH_PHY_XFP_FIBER;
                         break;
                 case PORT_HW_CFG_NET_SERDES_IF_SFI:
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index d5648fc666bd..bd1fd3d87c24 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -6794,8 +6794,9 @@ static int bnx2x_init_hw_port(struct bnx2x *bp)
 
         bnx2x_init_block(bp, BLOCK_DORQ, init_phase);
 
+        bnx2x_init_block(bp, BLOCK_BRB1, init_phase);
+
         if (CHIP_IS_E1(bp) || CHIP_IS_E1H(bp)) {
-                bnx2x_init_block(bp, BLOCK_BRB1, init_phase);
 
                 if (IS_MF(bp))
                         low = ((bp->flags & ONE_PORT_FLAG) ? 160 : 246);
@@ -11902,7 +11903,15 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
         /* disable FCOE L2 queue for E1x */
         if (CHIP_IS_E1x(bp))
                 bp->flags |= NO_FCOE_FLAG;
-
+        /* disable FCOE for 57840 device, until FW supports it */
+        switch (ent->driver_data) {
+        case BCM57840_O:
+        case BCM57840_4_10:
+        case BCM57840_2_20:
+        case BCM57840_MFO:
+        case BCM57840_MF:
+                bp->flags |= NO_FCOE_FLAG;
+        }
 #endif
 
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index 71971a161bd1..614981c02264 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -126,7 +126,7 @@ static inline int bnx2x_exe_queue_add(struct bnx2x *bp,
                 /* Check if this request is ok */
                 rc = o->validate(bp, o->owner, elem);
                 if (rc) {
-                        BNX2X_ERR("Preamble failed: %d\n", rc);
+                        DP(BNX2X_MSG_SP, "Preamble failed: %d\n", rc);
                         goto free_and_exit;
                 }
         }
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index 2b4b4f529ab4..16814b34d4b6 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -375,7 +375,6 @@ struct xgmac_priv {
         unsigned int tx_tail;
 
         void __iomem *base;
-        struct sk_buff_head rx_recycle;
         unsigned int dma_buf_sz;
         dma_addr_t dma_rx_phy;
         dma_addr_t dma_tx_phy;
@@ -672,9 +671,7 @@ static void xgmac_rx_refill(struct xgmac_priv *priv)
                 p = priv->dma_rx + entry;
 
                 if (priv->rx_skbuff[entry] == NULL) {
-                        skb = __skb_dequeue(&priv->rx_recycle);
-                        if (skb == NULL)
-                                skb = netdev_alloc_skb(priv->dev, priv->dma_buf_sz);
+                        skb = netdev_alloc_skb(priv->dev, priv->dma_buf_sz);
                         if (unlikely(skb == NULL))
                                 break;
 
@@ -887,17 +884,7 @@ static void xgmac_tx_complete(struct xgmac_priv *priv)
                                        desc_get_buf_len(p), DMA_TO_DEVICE);
                 }
 
-                /*
-                 * If there's room in the queue (limit it to size)
-                 * we add this skb back into the pool,
-                 * if it's the right size.
-                 */
-                if ((skb_queue_len(&priv->rx_recycle) <
-                        DMA_RX_RING_SZ) &&
-                        skb_recycle_check(skb, priv->dma_buf_sz))
-                        __skb_queue_head(&priv->rx_recycle, skb);
-                else
-                        dev_kfree_skb(skb);
+                dev_kfree_skb(skb);
         }
 
         if (dma_ring_space(priv->tx_head, priv->tx_tail, DMA_TX_RING_SZ) >
@@ -1016,7 +1003,6 @@ static int xgmac_open(struct net_device *dev)
                         dev->dev_addr);
         }
 
-        skb_queue_head_init(&priv->rx_recycle);
         memset(&priv->xstats, 0, sizeof(struct xgmac_extra_stats));
 
         /* Initialize the XGMAC and descriptors */
@@ -1053,7 +1039,6 @@ static int xgmac_stop(struct net_device *dev)
                 napi_disable(&priv->napi);
 
         writel(0, priv->base + XGMAC_DMA_INTR_ENA);
-        skb_queue_purge(&priv->rx_recycle);
 
         /* Disable the MAC core */
         xgmac_mac_disable(priv->base);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 31752b24434e..378988b5709a 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -251,6 +251,8 @@ struct adapter_params {
         unsigned char rev;                /* chip revision */
         unsigned char offload;
 
+        unsigned char bypass;
+
         unsigned int ofldq_wr_cred;
 };
 
@@ -642,6 +644,23 @@ extern int dbfifo_int_thresh;
 #define for_each_port(adapter, iter) \
         for (iter = 0; iter < (adapter)->params.nports; ++iter)
 
+static inline int is_bypass(struct adapter *adap)
+{
+        return adap->params.bypass;
+}
+
+static inline int is_bypass_device(int device)
+{
+        /* this should be set based upon device capabilities */
+        switch (device) {
+        case 0x440b:
+        case 0x440c:
+                return 1;
+        default:
+                return 0;
+        }
+}
+
 static inline unsigned int core_ticks_per_usec(const struct adapter *adap)
 {
         return adap->params.vpd.cclk / 1000;
@@ -696,6 +715,7 @@ int t4_seeprom_wp(struct adapter *adapter, bool enable);
 int get_vpd_params(struct adapter *adapter, struct vpd_params *p);
 int t4_load_fw(struct adapter *adapter, const u8 *fw_data, unsigned int size);
 unsigned int t4_flash_cfg_addr(struct adapter *adapter);
+int t4_load_cfg(struct adapter *adapter, const u8 *cfg_data, unsigned int size);
 int t4_check_fw_version(struct adapter *adapter);
 int t4_prep_adapter(struct adapter *adapter);
 int t4_port_init(struct adapter *adap, int mbox, int pf, int vf);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 6b9f6bb2f7ed..0df1284df497 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -443,7 +443,10 @@ int dbfifo_int_thresh = 10; /* 10 == 640 entry threshold */
 module_param(dbfifo_int_thresh, int, 0644);
 MODULE_PARM_DESC(dbfifo_int_thresh, "doorbell fifo interrupt threshold");
 
-int dbfifo_drain_delay = 1000; /* usecs to sleep while draining the dbfifo */
+/*
+ * usecs to sleep while draining the dbfifo
+ */
+static int dbfifo_drain_delay = 1000;
 module_param(dbfifo_drain_delay, int, 0644);
 MODULE_PARM_DESC(dbfifo_drain_delay,
                  "usecs to sleep while draining the dbfifo");
@@ -636,7 +639,7 @@ static void name_msix_vecs(struct adapter *adap)
 static int request_msix_queue_irqs(struct adapter *adap)
 {
         struct sge *s = &adap->sge;
-        int err, ethqidx, ofldqidx = 0, rdmaqidx = 0, msi = 2;
+        int err, ethqidx, ofldqidx = 0, rdmaqidx = 0, msi_index = 2;
 
         err = request_irq(adap->msix_info[1].vec, t4_sge_intr_msix, 0,
                           adap->msix_info[1].desc, &s->fw_evtq);
@@ -644,56 +647,60 @@ static int request_msix_queue_irqs(struct adapter *adap)
                 return err;
 
         for_each_ethrxq(s, ethqidx) {
-                err = request_irq(adap->msix_info[msi].vec, t4_sge_intr_msix, 0,
-                                  adap->msix_info[msi].desc,
+                err = request_irq(adap->msix_info[msi_index].vec,
+                                  t4_sge_intr_msix, 0,
+                                  adap->msix_info[msi_index].desc,
                                   &s->ethrxq[ethqidx].rspq);
                 if (err)
                         goto unwind;
-                msi++;
+                msi_index++;
         }
         for_each_ofldrxq(s, ofldqidx) {
-                err = request_irq(adap->msix_info[msi].vec, t4_sge_intr_msix, 0,
-                                  adap->msix_info[msi].desc,
+                err = request_irq(adap->msix_info[msi_index].vec,
+                                  t4_sge_intr_msix, 0,
+                                  adap->msix_info[msi_index].desc,
                                   &s->ofldrxq[ofldqidx].rspq);
                 if (err)
                         goto unwind;
-                msi++;
+                msi_index++;
         }
         for_each_rdmarxq(s, rdmaqidx) {
-                err = request_irq(adap->msix_info[msi].vec, t4_sge_intr_msix, 0,
-                                  adap->msix_info[msi].desc,
+                err = request_irq(adap->msix_info[msi_index].vec,
+                                  t4_sge_intr_msix, 0,
+                                  adap->msix_info[msi_index].desc,
                                   &s->rdmarxq[rdmaqidx].rspq);
                 if (err)
                         goto unwind;
-                msi++;
+                msi_index++;
         }
         return 0;
 
 unwind:
         while (--rdmaqidx >= 0)
-                free_irq(adap->msix_info[--msi].vec,
+                free_irq(adap->msix_info[--msi_index].vec,
                          &s->rdmarxq[rdmaqidx].rspq);
         while (--ofldqidx >= 0)
-                free_irq(adap->msix_info[--msi].vec,
+                free_irq(adap->msix_info[--msi_index].vec,
                          &s->ofldrxq[ofldqidx].rspq);
         while (--ethqidx >= 0)
-                free_irq(adap->msix_info[--msi].vec, &s->ethrxq[ethqidx].rspq);
+                free_irq(adap->msix_info[--msi_index].vec,
+                         &s->ethrxq[ethqidx].rspq);
         free_irq(adap->msix_info[1].vec, &s->fw_evtq);
         return err;
 }
 
 static void free_msix_queue_irqs(struct adapter *adap)
 {
-        int i, msi = 2;
+        int i, msi_index = 2;
         struct sge *s = &adap->sge;
 
         free_irq(adap->msix_info[1].vec, &s->fw_evtq);
         for_each_ethrxq(s, i)
-                free_irq(adap->msix_info[msi++].vec, &s->ethrxq[i].rspq);
+                free_irq(adap->msix_info[msi_index++].vec, &s->ethrxq[i].rspq);
         for_each_ofldrxq(s, i)
-                free_irq(adap->msix_info[msi++].vec, &s->ofldrxq[i].rspq);
+                free_irq(adap->msix_info[msi_index++].vec, &s->ofldrxq[i].rspq);
         for_each_rdmarxq(s, i)
-                free_irq(adap->msix_info[msi++].vec, &s->rdmarxq[i].rspq);
+                free_irq(adap->msix_info[msi_index++].vec, &s->rdmarxq[i].rspq);
 }
 
 /**
@@ -2535,9 +2542,8 @@ static int read_eq_indices(struct adapter *adap, u16 qid, u16 *pidx, u16 *cidx)
 
         ret = t4_mem_win_read_len(adap, addr, (__be32 *)&indices, 8);
         if (!ret) {
-                indices = be64_to_cpu(indices);
-                *cidx = (indices >> 25) & 0xffff;
-                *pidx = (indices >> 9) & 0xffff;
+                *cidx = (be64_to_cpu(indices) >> 25) & 0xffff;
+                *pidx = (be64_to_cpu(indices) >> 9) & 0xffff;
         }
         return ret;
 }
@@ -3410,16 +3416,6 @@ static int adap_init0_config(struct adapter *adapter, int reset)
                          finicsum, cfcsum);
 
         /*
-         * If we're a pure NIC driver then disable all offloading facilities.
-         * This will allow the firmware to optimize aspects of the hardware
-         * configuration which will result in improved performance.
-         */
-        caps_cmd.ofldcaps = 0;
-        caps_cmd.iscsicaps = 0;
-        caps_cmd.rdmacaps = 0;
-        caps_cmd.fcoecaps = 0;
-
-        /*
          * And now tell the firmware to use the configuration we just loaded.
          */
         caps_cmd.op_to_write =
@@ -3507,18 +3503,6 @@ static int adap_init0_no_config(struct adapter *adapter, int reset)
         if (ret < 0)
                 goto bye;
 
-#ifndef CONFIG_CHELSIO_T4_OFFLOAD
-        /*
-         * If we're a pure NIC driver then disable all offloading facilities.
-         * This will allow the firmware to optimize aspects of the hardware
-         * configuration which will result in improved performance.
-         */
-        caps_cmd.ofldcaps = 0;
-        caps_cmd.iscsicaps = 0;
-        caps_cmd.rdmacaps = 0;
-        caps_cmd.fcoecaps = 0;
-#endif
-
         if (caps_cmd.niccaps & htons(FW_CAPS_CONFIG_NIC_VM)) {
                 if (!vf_acls)
                         caps_cmd.niccaps ^= htons(FW_CAPS_CONFIG_NIC_VM);
@@ -3634,10 +3618,10 @@ static int adap_init0_no_config(struct adapter *adapter, int reset)
          * field selections will fit in the 36-bit budget.
          */
         if (tp_vlan_pri_map != TP_VLAN_PRI_MAP_DEFAULT) {
-                int i, bits = 0;
+                int j, bits = 0;
 
-                for (i = TP_VLAN_PRI_MAP_FIRST; i <= TP_VLAN_PRI_MAP_LAST; i++)
-                        switch (tp_vlan_pri_map & (1 << i)) {
+                for (j = TP_VLAN_PRI_MAP_FIRST; j <= TP_VLAN_PRI_MAP_LAST; j++)
+                        switch (tp_vlan_pri_map & (1 << j)) {
                         case 0:
                                 /* compressed filter field not enabled */
                                 break;
@@ -3739,6 +3723,7 @@ static int adap_init0(struct adapter *adap)
         u32 v, port_vec;
         enum dev_state state;
         u32 params[7], val[7];
+        struct fw_caps_config_cmd caps_cmd;
         int reset = 1, j;
 
         /*
@@ -3892,6 +3877,9 @@ static int adap_init0(struct adapter *adap)
                         goto bye;
         }
 
+        if (is_bypass_device(adap->pdev->device))
+                adap->params.bypass = 1;
+
         /*
          * Grab some of our basic fundamental operating parameters.
          */
@@ -3934,13 +3922,12 @@ static int adap_init0(struct adapter *adap)
                 adap->tids.aftid_end = val[1];
         }
 
-#ifdef CONFIG_CHELSIO_T4_OFFLOAD
         /*
          * Get device capabilities so we can determine what resources we need
          * to manage.
          */
         memset(&caps_cmd, 0, sizeof(caps_cmd));
-        caps_cmd.op_to_write = htonl(V_FW_CMD_OP(FW_CAPS_CONFIG_CMD) |
+        caps_cmd.op_to_write = htonl(FW_CMD_OP(FW_CAPS_CONFIG_CMD) |
                                      FW_CMD_REQUEST | FW_CMD_READ);
         caps_cmd.retval_len16 = htonl(FW_LEN16(caps_cmd));
         ret = t4_wr_mbox(adap, adap->mbox, &caps_cmd, sizeof(caps_cmd),
@@ -3985,15 +3972,6 @@ static int adap_init0(struct adapter *adap)
                 adap->vres.ddp.size = val[4] - val[3] + 1;
                 adap->params.ofldq_wr_cred = val[5];
 
-                params[0] = FW_PARAM_PFVF(ETHOFLD_START);
-                params[1] = FW_PARAM_PFVF(ETHOFLD_END);
-                ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2,
-                                      params, val);
-                if ((val[0] != val[1]) && (ret >= 0)) {
-                        adap->tids.uotid_base = val[0];
-                        adap->tids.nuotids = val[1] - val[0] + 1;
-                }
-
                 adap->params.offload = 1;
         }
         if (caps_cmd.rdmacaps) {
@@ -4042,7 +4020,6 @@ static int adap_init0(struct adapter *adap)
         }
 #undef FW_PARAM_PFVF
 #undef FW_PARAM_DEV
-#endif /* CONFIG_CHELSIO_T4_OFFLOAD */
 
         /*
          * These are finalized by FW initialization, load their values now.
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index 1b899fea1a91..39bec73ff87c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -102,6 +102,9 @@ struct tid_info {
         unsigned int ftid_base;
         unsigned int aftid_base;
         unsigned int aftid_end;
+        /* Server filter region */
+        unsigned int sftid_base;
+        unsigned int nsftids;
 
         spinlock_t atid_lock ____cacheline_aligned_in_smp;
         union aopen_entry *afree;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 137a24438d9c..32eec15fe4c2 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -380,9 +380,11 @@ static int t4_mem_win_rw(struct adapter *adap, u32 addr, __be32 *data, int dir)
         /* Collecting data 4 bytes at a time upto MEMWIN0_APERTURE */
         for (i = 0; i < MEMWIN0_APERTURE; i = i+0x4) {
                 if (dir)
-                        *data++ = t4_read_reg(adap, (MEMWIN0_BASE + i));
+                        *data++ = (__force __be32) t4_read_reg(adap,
+                                                        (MEMWIN0_BASE + i));
                 else
-                        t4_write_reg(adap, (MEMWIN0_BASE + i), *data++);
+                        t4_write_reg(adap, (MEMWIN0_BASE + i),
+                                     (__force u32) *data++);
         }
 
         return 0;
@@ -417,7 +419,7 @@ static int t4_memory_rw(struct adapter *adap, int mtype, u32 addr, u32 len,
         if ((addr & 0x3) || (len & 0x3))
                 return -EINVAL;
 
-        data = vmalloc(MEMWIN0_APERTURE/sizeof(__be32));
+        data = vmalloc(MEMWIN0_APERTURE);
         if (!data)
                 return -ENOMEM;
 
@@ -744,7 +746,7 @@ static int t4_read_flash(struct adapter *adapter, unsigned int addr,
                 if (ret)
                         return ret;
                 if (byte_oriented)
-                        *data = htonl(*data);
+                        *data = (__force __u32) (htonl(*data));
         }
         return 0;
 }
@@ -992,7 +994,7 @@ int t4_load_fw(struct adapter *adap, const u8 *fw_data, unsigned int size)
         int ret, addr;
         unsigned int i;
         u8 first_page[SF_PAGE_SIZE];
-        const u32 *p = (const u32 *)fw_data;
+        const __be32 *p = (const __be32 *)fw_data;
         const struct fw_hdr *hdr = (const struct fw_hdr *)fw_data;
         unsigned int sf_sec_size = adap->params.sf_size / adap->params.sf_nsec;
         unsigned int fw_img_start = adap->params.sf_fw_start;
@@ -2315,7 +2317,8 @@ int t4_mem_win_read_len(struct adapter *adap, u32 addr, __be32 *data, int len)
         t4_read_reg(adap, PCIE_MEM_ACCESS_OFFSET);
 
         for (i = 0; i < len; i += 4)
-                *data++ = t4_read_reg(adap, (MEMWIN0_BASE + off + i));
+                *data++ = (__force __be32) t4_read_reg(adap,
+                                                (MEMWIN0_BASE + off + i));
 
         return 0;
 }
diff --git a/drivers/net/ethernet/dec/tulip/dmfe.c b/drivers/net/ethernet/dec/tulip/dmfe.c
index 4d6fe604fa64..d23755ea9bc7 100644
--- a/drivers/net/ethernet/dec/tulip/dmfe.c
+++ b/drivers/net/ethernet/dec/tulip/dmfe.c
@@ -446,13 +446,17 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
         /* Allocate Tx/Rx descriptor memory */
         db->desc_pool_ptr = pci_alloc_consistent(pdev, sizeof(struct tx_desc) *
                         DESC_ALL_CNT + 0x20, &db->desc_pool_dma_ptr);
-        if (!db->desc_pool_ptr)
+        if (!db->desc_pool_ptr) {
+                err = -ENOMEM;
                 goto err_out_res;
+        }
 
         db->buf_pool_ptr = pci_alloc_consistent(pdev, TX_BUF_ALLOC *
                         TX_DESC_CNT + 4, &db->buf_pool_dma_ptr);
-        if (!db->buf_pool_ptr)
+        if (!db->buf_pool_ptr) {
+                err = -ENOMEM;
                 goto err_out_free_desc;
+        }
 
         db->first_tx_desc = (struct tx_desc *) db->desc_pool_ptr;
         db->first_tx_desc_dma = db->desc_pool_dma_ptr;
@@ -462,8 +466,10 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
         db->chip_id = ent->driver_data;
         /* IO type range. */
         db->ioaddr = pci_iomap(pdev, 0, 0);
-        if (!db->ioaddr)
+        if (!db->ioaddr) {
+                err = -ENOMEM;
                 goto err_out_free_buf;
+        }
 
         db->chip_revision = pdev->revision;
         db->wol_mode = 0;
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index eb3f2cb3b93b..d1b6cc587639 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -2129,8 +2129,11 @@ void be_detect_error(struct be_adapter *adapter)
                 ue_hi = (ue_hi & ~ue_hi_mask);
         }
 
-        if (ue_lo || ue_hi ||
-                sliport_status & SLIPORT_STATUS_ERR_MASK) {
+        /* On certain platforms BE hardware can indicate spurious UEs.
+         * Allow the h/w to stop working completely in case of a real UE.
+         * Hence not setting the hw_error for UE detection.
+         */
+        if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
                 adapter->hw_error = true;
                 dev_err(&adapter->pdev->dev,
                         "Error detected in the card\n");
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index a1b52ec3b930..1d03dcdd5e56 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1765,7 +1765,6 @@ static void free_skb_resources(struct gfar_private *priv)
                           sizeof(struct rxbd8) * priv->total_rx_ring_size,
                           priv->tx_queue[0]->tx_bd_base,
                           priv->tx_queue[0]->tx_bd_dma_base);
-        skb_queue_purge(&priv->rx_recycle);
 }
 
 void gfar_start(struct net_device *dev)
@@ -1943,8 +1942,6 @@ static int gfar_enet_open(struct net_device *dev)
 
         enable_napi(priv);
 
-        skb_queue_head_init(&priv->rx_recycle);
-
         /* Initialize a bunch of registers */
         init_registers(dev);
 
@@ -2533,16 +2530,7 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
                 bytes_sent += skb->len;
 
-                /* If there's room in the queue (limit it to rx_buffer_size)
-                 * we add this skb back into the pool, if it's the right size
-                 */
-                if (skb_queue_len(&priv->rx_recycle) < rx_queue->rx_ring_size &&
-                    skb_recycle_check(skb, priv->rx_buffer_size +
-                                      RXBUF_ALIGNMENT)) {
-                        gfar_align_skb(skb);
-                        skb_queue_head(&priv->rx_recycle, skb);
-                } else
-                        dev_kfree_skb_any(skb);
+                dev_kfree_skb_any(skb);
 
                 tx_queue->tx_skbuff[skb_dirtytx] = NULL;
 
@@ -2608,7 +2596,7 @@ static void gfar_new_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
 static struct sk_buff *gfar_alloc_skb(struct net_device *dev)
 {
         struct gfar_private *priv = netdev_priv(dev);
-        struct sk_buff *skb = NULL;
+        struct sk_buff *skb;
 
         skb = netdev_alloc_skb(dev, priv->rx_buffer_size + RXBUF_ALIGNMENT);
         if (!skb)
@@ -2621,14 +2609,7 @@ static struct sk_buff *gfar_alloc_skb(struct net_device *dev)
 
 struct sk_buff *gfar_new_skb(struct net_device *dev)
 {
-        struct gfar_private *priv = netdev_priv(dev);
-        struct sk_buff *skb = NULL;
-
-        skb = skb_dequeue(&priv->rx_recycle);
-        if (!skb)
-                skb = gfar_alloc_skb(dev);
-
-        return skb;
+        return gfar_alloc_skb(dev);
 }
 
 static inline void count_errors(unsigned short status, struct net_device *dev)
@@ -2787,7 +2768,7 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
                         if (unlikely(!newskb))
                                 newskb = skb;
                         else if (skb)
-                                skb_queue_head(&priv->rx_recycle, skb);
+                                dev_kfree_skb(skb);
                 } else {
                         /* Increment the number of packets */
                         rx_queue->stats.rx_packets++;
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 4141ef2ddafc..22eabc13ca99 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -1080,8 +1080,6 @@ struct gfar_private {
 
         u32 cur_filer_idx;
 
-        struct sk_buff_head rx_recycle;
-
         /* RX queue filer rule set*/
         struct ethtool_rx_list rx_list;
         struct mutex rx_queue_access;
diff --git a/drivers/net/ethernet/freescale/gianfar_ptp.c b/drivers/net/ethernet/freescale/gianfar_ptp.c
index b9db0e040563..2e5daee0438a 100644
--- a/drivers/net/ethernet/freescale/gianfar_ptp.c
+++ b/drivers/net/ethernet/freescale/gianfar_ptp.c
@@ -478,7 +478,7 @@ static int gianfar_ptp_probe(struct platform_device *dev)
                 pr_err("no resource\n");
                 goto no_resource;
         }
-        if (request_resource(&ioport_resource, etsects->rsrc)) {
+        if (request_resource(&iomem_resource, etsects->rsrc)) {
                 pr_err("resource busy\n");
                 goto no_resource;
         }
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 164288439220..0a70bb55d1b0 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -209,14 +209,12 @@ static struct list_head *dequeue(struct list_head *lh)
 static struct sk_buff *get_new_skb(struct ucc_geth_private *ugeth,
                 u8 __iomem *bd)
 {
-        struct sk_buff *skb = NULL;
+        struct sk_buff *skb;
 
-        skb = __skb_dequeue(&ugeth->rx_recycle);
+        skb = netdev_alloc_skb(ugeth->ndev,
+                               ugeth->ug_info->uf_info.max_rx_buf_length +
+                               UCC_GETH_RX_DATA_BUF_ALIGNMENT);
         if (!skb)
-                skb = netdev_alloc_skb(ugeth->ndev,
-                                      ugeth->ug_info->uf_info.max_rx_buf_length +
-                                      UCC_GETH_RX_DATA_BUF_ALIGNMENT);
-        if (skb == NULL)
                 return NULL;
 
         /* We need the data buffer to be aligned properly.  We will reserve
@@ -2020,8 +2018,6 @@ static void ucc_geth_memclean(struct ucc_geth_private *ugeth)
                 iounmap(ugeth->ug_regs);
                 ugeth->ug_regs = NULL;
         }
-
-        skb_queue_purge(&ugeth->rx_recycle);
 }
 
 static void ucc_geth_set_multi(struct net_device *dev)
@@ -2230,8 +2226,6 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
                 return -ENOMEM;
         }
 
-        skb_queue_head_init(&ugeth->rx_recycle);
-
         return 0;
 }
 
@@ -3274,12 +3268,7 @@ static int ucc_geth_rx(struct ucc_geth_private *ugeth, u8 rxQ, int rx_work_limit
                         if (netif_msg_rx_err(ugeth))
                                 ugeth_err("%s, %d: ERROR!!! skb - 0x%08x",
                                            __func__, __LINE__, (u32) skb);
-                        if (skb) {
-                                skb->data = skb->head + NET_SKB_PAD;
-                                skb->len = 0;
-                                skb_reset_tail_pointer(skb);
-                                __skb_queue_head(&ugeth->rx_recycle, skb);
-                        }
+                        dev_kfree_skb(skb);
 
                         ugeth->rx_skbuff[rxQ][ugeth->skb_currx[rxQ]] = NULL;
                         dev->stats.rx_dropped++;
@@ -3349,13 +3338,7 @@ static int ucc_geth_tx(struct net_device *dev, u8 txQ)
 
                 dev->stats.tx_packets++;
 
-                if (skb_queue_len(&ugeth->rx_recycle) < RX_BD_RING_LEN &&
-                             skb_recycle_check(skb,
-                                    ugeth->ug_info->uf_info.max_rx_buf_length +
-                                    UCC_GETH_RX_DATA_BUF_ALIGNMENT))
-                        __skb_queue_head(&ugeth->rx_recycle, skb);
-                else
-                        dev_kfree_skb(skb);
+                dev_kfree_skb(skb);
 
                 ugeth->tx_skbuff[txQ][ugeth->skb_dirtytx[txQ]] = NULL;
                 ugeth->skb_dirtytx[txQ] =
diff --git a/drivers/net/ethernet/freescale/ucc_geth.h b/drivers/net/ethernet/freescale/ucc_geth.h
index f71b3e7b12de..75f337163ce3 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.h
+++ b/drivers/net/ethernet/freescale/ucc_geth.h
@@ -1214,8 +1214,6 @@ struct ucc_geth_private {
         /* index of the first skb which hasn't been transmitted yet. */
         u16 skb_dirtytx[NUM_TX_QUEUES];
 
-        struct sk_buff_head rx_recycle;
-
         struct ugeth_mii_info *mii_info;
         struct phy_device *phydev;
         phy_interface_t phy_interface;
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index cb3356c9af80..04668b47a1df 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -175,13 +175,13 @@ struct e1000_info;
 /*
  * in the case of WTHRESH, it appears at least the 82571/2 hardware
  * writes back 4 descriptors when WTHRESH=5, and 3 descriptors when
- * WTHRESH=4, and since we want 64 bytes at a time written back, set
- * it to 5
+ * WTHRESH=4, so a setting of 5 gives the most efficient bus
+ * utilization but to avoid possible Tx stalls, set it to 1
  */
 #define E1000_TXDCTL_DMA_BURST_ENABLE                          \
         (E1000_TXDCTL_GRAN | /* set descriptor granularity */  \
          E1000_TXDCTL_COUNT_DESC |                             \
-         (5 << 16) | /* wthresh must be +1 more than desired */\
+         (1 << 16) | /* wthresh must be +1 more than desired */\
          (1 << 8)  | /* hthresh */                             \
          0x1f)       /* pthresh */
 
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index ed5b40985edb..d37bfd96c987 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -412,6 +412,8 @@ enum e1e_registers {
 #define E1000_DEV_ID_PCH2_LV_V                  0x1503
 #define E1000_DEV_ID_PCH_LPT_I217_LM            0x153A
 #define E1000_DEV_ID_PCH_LPT_I217_V             0x153B
+#define E1000_DEV_ID_PCH_LPTLP_I218_LM          0x155A
+#define E1000_DEV_ID_PCH_LPTLP_I218_V           0x1559
 
 #define E1000_REVISION_4 4
 
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index fb659dd8db03..f444eb0b76d8 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -2831,7 +2831,7 @@ static void e1000_configure_tx(struct e1000_adapter *adapter)
                  * set up some performance related parameters to encourage the
                  * hardware to use the bus more efficiently in bursts, depends
                  * on the tx_int_delay to be enabled,
-                 * wthresh = 5 ==> burst write a cacheline (64 bytes) at a time
+                 * wthresh = 1 ==> burst write is disabled to avoid Tx stalls
                  * hthresh = 1 ==> prefetch when one or more available
                  * pthresh = 0x1f ==> prefetch if internal cache 31 or less
                  * BEWARE: this seems to work but should be considered first if
@@ -6558,6 +6558,8 @@ static DEFINE_PCI_DEVICE_TABLE(e1000_pci_tbl) = {
 
         { PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPT_I217_LM), board_pch_lpt },
         { PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPT_I217_V), board_pch_lpt },
+        { PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPTLP_I218_LM), board_pch_lpt },
+        { PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPTLP_I218_V), board_pch_lpt },
 
         { 0, 0, 0, 0, 0, 0, 0 } /* terminate list */
 };
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 5bd26763554c..30efc9f0f47a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -410,7 +410,7 @@ static inline u16 ixgbe_desc_unused(struct ixgbe_ring *ring)
 #define IXGBE_TX_CTXTDESC(R, i)     \
         (&(((struct ixgbe_adv_tx_context_desc *)((R)->desc))[i]))
 
-#define IXGBE_MAX_JUMBO_FRAME_SIZE        16128
+#define IXGBE_MAX_JUMBO_FRAME_SIZE      9728 /* Maximum Supported Size 9.5KB */
 #ifdef IXGBE_FCOE
 /* Use 3K as the baby jumbo frame size for FCoE */
 #define IXGBE_FCOE_JUMBO_FRAME_SIZE       3072
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index 56b20d17d0e4..116f0e901bee 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -2673,6 +2673,9 @@ static int ixgbe_get_ts_info(struct net_device *dev,
         case ixgbe_mac_X540:
         case ixgbe_mac_82599EB:
                 info->so_timestamping =
+                        SOF_TIMESTAMPING_TX_SOFTWARE |
+                        SOF_TIMESTAMPING_RX_SOFTWARE |
+                        SOF_TIMESTAMPING_SOFTWARE |
                         SOF_TIMESTAMPING_TX_HARDWARE |
                         SOF_TIMESTAMPING_RX_HARDWARE |
                         SOF_TIMESTAMPING_RAW_HARDWARE;
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index 383b4e1cd175..4a9c9c285685 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -175,7 +175,7 @@ struct ixgbevf_q_vector {
 #define IXGBEVF_TX_CTXTDESC(R, i)           \
         (&(((struct ixgbe_adv_tx_context_desc *)((R)->desc))[i]))
 
-#define IXGBE_MAX_JUMBO_FRAME_SIZE        16128
+#define IXGBE_MAX_JUMBO_FRAME_SIZE      9728 /* Maximum Supported Size 9.5KB */
 
 #define OTHER_VECTOR 1
 #define NON_Q_VECTORS (OTHER_VECTOR)
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 0ee9bd4819f4..de1ad506665d 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -1747,6 +1747,7 @@ err_tx_ring_allocation:
  **/
 static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 {
+        struct net_device *netdev = adapter->netdev;
         int err = 0;
         int vector, v_budget;
 
@@ -1775,6 +1776,12 @@ static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 
         ixgbevf_acquire_msix_vectors(adapter, v_budget);
 
+        err = netif_set_real_num_tx_queues(netdev, adapter->num_tx_queues);
+        if (err)
+                goto out;
+
+        err = netif_set_real_num_rx_queues(netdev, adapter->num_rx_queues);
+
 out:
         return err;
 }
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index c911d883c27e..f8064df10cc4 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -27,6 +27,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
+#include <linux/pci-aspm.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/ethtool.h>
@@ -2973,6 +2974,9 @@ jme_init_one(struct pci_dev *pdev,
         /*
          * set up PCI device basics
          */
+        pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1 |
+                               PCIE_LINK_STATE_CLKPM);
+
         rc = pci_enable_device(pdev);
         if (rc) {
                 pr_err("Cannot enable PCI device\n");
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 087b9e0669f1..84c13263c514 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -412,7 +412,6 @@ struct mv643xx_eth_private {
         u8 work_rx_refill;
 
         int skb_size;
-        struct sk_buff_head rx_recycle;
 
         /*
          * RX state.
@@ -673,9 +672,7 @@ static int rxq_refill(struct rx_queue *rxq, int budget)
                 struct rx_desc *rx_desc;
                 int size;
 
-                skb = __skb_dequeue(&mp->rx_recycle);
-                if (skb == NULL)
-                        skb = netdev_alloc_skb(mp->dev, mp->skb_size);
+                skb = netdev_alloc_skb(mp->dev, mp->skb_size);
 
                 if (skb == NULL) {
                         mp->oom = 1;
@@ -989,14 +986,7 @@ static int txq_reclaim(struct tx_queue *txq, int budget, int force)
                                        desc->byte_cnt, DMA_TO_DEVICE);
                 }
 
-                if (skb != NULL) {
-                        if (skb_queue_len(&mp->rx_recycle) <
-                                        mp->rx_ring_size &&
-                            skb_recycle_check(skb, mp->skb_size))
-                                __skb_queue_head(&mp->rx_recycle, skb);
-                        else
-                                dev_kfree_skb(skb);
-                }
+                dev_kfree_skb(skb);
         }
 
         __netif_tx_unlock(nq);
@@ -2349,8 +2339,6 @@ static int mv643xx_eth_open(struct net_device *dev)
 
         napi_enable(&mp->napi);
 
-        skb_queue_head_init(&mp->rx_recycle);
-
         mp->int_mask = INT_EXT;
 
         for (i = 0; i < mp->rxq_count; i++) {
@@ -2445,8 +2433,6 @@ static int mv643xx_eth_stop(struct net_device *dev)
         mib_counters_update(mp);
         del_timer_sync(&mp->mib_counters_timer);
 
-        skb_queue_purge(&mp->rx_recycle);
-
         for (i = 0; i < mp->rxq_count; i++)
                 rxq_deinit(mp->rxq + i);
         for (i = 0; i < mp->txq_count; i++)
diff --git a/drivers/net/ethernet/marvell/skge.c b/drivers/net/ethernet/marvell/skge.c
index 5a30bf823099..9b9c2ac5c4c2 100644
--- a/drivers/net/ethernet/marvell/skge.c
+++ b/drivers/net/ethernet/marvell/skge.c
@@ -3189,7 +3189,7 @@ static int skge_poll(struct napi_struct *napi, int to_do)
         if (work_done < to_do) {
                 unsigned long flags;
 
-                napi_gro_flush(napi);
+                napi_gro_flush(napi, false);
                 spin_lock_irqsave(&hw->hw_lock, flags);
                 __napi_complete(napi);
                 hw->intr_mask |= napimask[skge->port];
@@ -3945,8 +3945,10 @@ static int __devinit skge_probe(struct pci_dev *pdev,
                 skge_board_name(hw), hw->chip_rev);
 
         dev = skge_devinit(hw, 0, using_dac);
-        if (!dev)
+        if (!dev) {
+                err = -ENOMEM;
                 goto err_out_led_off;
+        }
 
         /* Some motherboards are broken and has zero in ROM. */
         if (!is_valid_ether_addr(dev->dev_addr))
@@ -4153,6 +4155,13 @@ static struct dmi_system_id skge_32bit_dma_boards[] = {
                         DMI_MATCH(DMI_BOARD_NAME, "nForce"),
                 },
         },
+        {
+                .ident = "ASUS P5NSLI",
+                .matches = {
+                        DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+                        DMI_MATCH(DMI_BOARD_NAME, "P5NSLI")
+                },
+        },
         {}
 };
 
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 2b0748dba8b8..78946feab4a2 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -4924,6 +4924,7 @@ static int __devinit sky2_probe(struct pci_dev *pdev,
 
         if (~reg == 0) {
                 dev_err(&pdev->dev, "PCI configuration read error\n");
+                err = -EIO;
                 goto err_out;
         }
 
@@ -4993,8 +4994,10 @@ static int __devinit sky2_probe(struct pci_dev *pdev,
         hw->st_size = hw->ports * roundup_pow_of_two(3*RX_MAX_PENDING + TX_MAX_PENDING);
         hw->st_le = pci_alloc_consistent(pdev, hw->st_size * sizeof(struct sky2_status_le),
                                          &hw->st_dma);
-        if (!hw->st_le)
+        if (!hw->st_le) {
+                err = -ENOMEM;
                 goto err_out_reset;
+        }
 
         dev_info(&pdev->dev, "Yukon-2 %s chip revision %d\n",
                  sky2_name(hw->chip_id, buf1, sizeof(buf1)), hw->chip_rev);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index c10e3a6de09f..b35094c590ba 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -143,7 +143,6 @@ void mlx4_en_destroy_tx_ring(struct mlx4_en_priv *priv,
                 mlx4_bf_free(mdev->dev, &ring->bf);
         mlx4_qp_remove(mdev->dev, &ring->qp);
         mlx4_qp_free(mdev->dev, &ring->qp);
-        mlx4_qp_release_range(mdev->dev, ring->qpn, 1);
         mlx4_en_unmap_buffer(&ring->wqres.buf);
         mlx4_free_hwq_res(mdev->dev, &ring->wqres, ring->buf_size);
         kfree(ring->bounce_buf);
@@ -712,7 +711,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
         if (bounce)
                 tx_desc = mlx4_en_bounce_to_desc(priv, ring, index, desc_size);
 
-        if (ring->bf_enabled && desc_size <= MAX_BF && !bounce && !vlan_tag) {
+        if (ring->bf_enabled && desc_size <= MAX_BF && !bounce && !vlan_tx_tag_present(skb)) {
                 *(__be32 *) (&tx_desc->ctrl.vlan_tag) |= cpu_to_be32(ring->doorbell_qpn);
                 op_own |= htonl((bf_index & 0xffff) << 8);
                 /* Ensure new descirptor hits memory
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 51c764901ad2..b84a88bc44dc 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -329,9 +329,6 @@ int set_and_calc_slave_port_state(struct mlx4_dev *dev, int slave,
         ctx = &priv->mfunc.master.slave_state[slave];
         spin_lock_irqsave(&ctx->lock, flags);
 
-        mlx4_dbg(dev, "%s: slave: %d, current state: %d new event :%d\n",
-                 __func__, slave, cur_state, event);
-
         switch (cur_state) {
         case SLAVE_PORT_DOWN:
                 if (MLX4_PORT_STATE_DEV_EVENT_PORT_UP == event)
@@ -366,9 +363,6 @@ int set_and_calc_slave_port_state(struct mlx4_dev *dev, int slave,
                         goto out;
         }
         ret = mlx4_get_slave_port_state(dev, slave, port);
-        mlx4_dbg(dev, "%s: slave: %d, current state: %d new event"
-                 " :%d gen_event: %d\n",
-                 __func__, slave, cur_state, event, *gen_event);
 
 out:
         spin_unlock_irqrestore(&ctx->lock, flags);
@@ -843,6 +837,18 @@ static void __iomem *mlx4_get_eq_uar(struct mlx4_dev *dev, struct mlx4_eq *eq)
         return priv->eq_table.uar_map[index] + 0x800 + 8 * (eq->eqn % 4);
 }
 
+static void mlx4_unmap_uar(struct mlx4_dev *dev)
+{
+        struct mlx4_priv *priv = mlx4_priv(dev);
+        int i;
+
+        for (i = 0; i < mlx4_num_eq_uar(dev); ++i)
+                if (priv->eq_table.uar_map[i]) {
+                        iounmap(priv->eq_table.uar_map[i]);
+                        priv->eq_table.uar_map[i] = NULL;
+                }
+}
+
 static int mlx4_create_eq(struct mlx4_dev *dev, int nent,
                           u8 intr, struct mlx4_eq *eq)
 {
@@ -1207,6 +1213,7 @@ err_out_unmap:
         mlx4_free_irqs(dev);
 
 err_out_bitmap:
+        mlx4_unmap_uar(dev);
         mlx4_bitmap_cleanup(&priv->eq_table.bitmap);
 
 err_out_free:
@@ -1231,10 +1238,7 @@ void mlx4_cleanup_eq_table(struct mlx4_dev *dev)
         if (!mlx4_is_slave(dev))
                 mlx4_unmap_clr_int(dev);
 
-        for (i = 0; i < mlx4_num_eq_uar(dev); ++i)
-                if (priv->eq_table.uar_map[i])
-                        iounmap(priv->eq_table.uar_map[i]);
-
+        mlx4_unmap_uar(dev);
         mlx4_bitmap_cleanup(&priv->eq_table.bitmap);
 
         kfree(priv->eq_table.uar_map);
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 80df2ab0177c..2aa80afd98d2 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -1405,7 +1405,10 @@ unmap_bf:
         unmap_bf_area(dev);
 
 err_close:
-        mlx4_close_hca(dev);
+        if (mlx4_is_slave(dev))
+                mlx4_slave_exit(dev);
+        else
+                mlx4_CLOSE_HCA(dev, 0);
 
 err_free_icm:
         if (!mlx4_is_slave(dev))
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 926c911c0ac4..b05705f50f0f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -330,9 +330,6 @@ static void update_pkey_index(struct mlx4_dev *dev, int slave,
 
         new_index = priv->virt2phys_pkey[slave][port - 1][orig_index];
         *(u8 *)(inbox->buf + 35) = new_index;
-
-        mlx4_dbg(dev, "port = %d, orig pkey index = %d, "
-                 "new pkey index = %d\n", port, orig_index, new_index);
 }
 
 static void update_gid(struct mlx4_dev *dev, struct mlx4_cmd_mailbox *inbox,
@@ -351,9 +348,6 @@ static void update_gid(struct mlx4_dev *dev, struct mlx4_cmd_mailbox *inbox,
                 if (optpar & MLX4_QP_OPTPAR_ALT_ADDR_PATH)
                         qp_ctx->alt_path.mgid_index = slave & 0x7F;
         }
-
-        mlx4_dbg(dev, "slave %d, new gid index: 0x%x ",
-                slave, qp_ctx->pri_path.mgid_index);
 }
 
 static int mpt_mask(struct mlx4_dev *dev)
diff --git a/drivers/net/ethernet/natsemi/natsemi.c b/drivers/net/ethernet/natsemi/natsemi.c
index 5b61d12f8b91..dbaaa99a0d43 100644
--- a/drivers/net/ethernet/natsemi/natsemi.c
+++ b/drivers/net/ethernet/natsemi/natsemi.c
@@ -947,8 +947,8 @@ static int __devinit natsemi_probe1 (struct pci_dev *pdev,
         i = register_netdev(dev);
         if (i)
                 goto err_register_netdev;
-
-        if (NATSEMI_CREATE_FILE(pdev, dspcfg_workaround))
+        i = NATSEMI_CREATE_FILE(pdev, dspcfg_workaround);
+        if (i)
                 goto err_create_file;
 
         if (netif_msg_drv(np)) {
diff --git a/drivers/net/ethernet/natsemi/xtsonic.c b/drivers/net/ethernet/natsemi/xtsonic.c
index e01c0a07a93a..7dfe88398d7d 100644
--- a/drivers/net/ethernet/natsemi/xtsonic.c
+++ b/drivers/net/ethernet/natsemi/xtsonic.c
@@ -205,6 +205,7 @@ static int __init sonic_probe1(struct net_device *dev)
         if (lp->descriptors == NULL) {
                 printk(KERN_ERR "%s: couldn't alloc DMA memory for "
                                 " descriptors.\n", dev_name(lp->device));
+                err = -ENOMEM;
                 goto out;
         }
 
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index 53743f7a2ca9..af8b4142088c 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -1524,6 +1524,7 @@ static int lpc_eth_drv_remove(struct platform_device *pdev)
                                   pldat->dma_buff_base_p);
         free_irq(ndev->irq, ndev);
         iounmap(pldat->net_base);
+        mdiobus_unregister(pldat->mii_bus);
         mdiobus_free(pldat->mii_bus);
         clk_disable(pldat->clk);
         clk_put(pldat->clk);
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig b/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
index 97302419a377..5296cc8d3cba 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
@@ -26,6 +26,9 @@ if PCH_GBE
 config PCH_PTP
         bool "PCH PTP clock support"
         default n
+        depends on EXPERIMENTAL
+        select PPS
+        select PTP_1588_CLOCK
         select PTP_1588_CLOCK_PCH
         ---help---
           Say Y here if you want to use Precision Time Protocol (PTP) in the
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
index b2a94d02a521..4c4fe5b1a29a 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
@@ -339,26 +339,6 @@ static void pch_gbe_wait_clr_bit(void *reg, u32 bit)
 }
 
 /**
- * pch_gbe_wait_clr_bit_irq - Wait to clear a bit for interrupt context
- * @reg:        Pointer of register
- * @busy:       Busy bit
- */
-static int pch_gbe_wait_clr_bit_irq(void *reg, u32 bit)
-{
-        u32 tmp;
-        int ret = -1;
-        /* wait busy */
-        tmp = 20;
-        while ((ioread32(reg) & bit) && --tmp)
-                udelay(5);
-        if (!tmp)
-                pr_err("Error: busy bit is not cleared\n");
-        else
-                ret = 0;
-        return ret;
-}
-
-/**
  * pch_gbe_mac_mar_set - Set MAC address register
  * @hw:     Pointer to the HW structure
  * @addr:   Pointer to the MAC address
@@ -409,15 +389,20 @@ static void pch_gbe_mac_reset_hw(struct pch_gbe_hw *hw)
         return;
 }
 
-static void pch_gbe_mac_reset_rx(struct pch_gbe_hw *hw)
+static void pch_gbe_disable_mac_rx(struct pch_gbe_hw *hw)
 {
-        /* Read the MAC addresses. and store to the private data */
-        pch_gbe_mac_read_mac_addr(hw);
-        iowrite32(PCH_GBE_RX_RST, &hw->reg->RESET);
-        pch_gbe_wait_clr_bit_irq(&hw->reg->RESET, PCH_GBE_RX_RST);
-        /* Setup the MAC addresses */
-        pch_gbe_mac_mar_set(hw, hw->mac.addr, 0);
-        return;
+        u32 rctl;
+        /* Disables Receive MAC */
+        rctl = ioread32(&hw->reg->MAC_RX_EN);
+        iowrite32((rctl & ~PCH_GBE_MRE_MAC_RX_EN), &hw->reg->MAC_RX_EN);
+}
+
+static void pch_gbe_enable_mac_rx(struct pch_gbe_hw *hw)
+{
+        u32 rctl;
+        /* Enables Receive MAC */
+        rctl = ioread32(&hw->reg->MAC_RX_EN);
+        iowrite32((rctl | PCH_GBE_MRE_MAC_RX_EN), &hw->reg->MAC_RX_EN);
 }
 
 /**
@@ -913,7 +898,7 @@ static void pch_gbe_setup_rctl(struct pch_gbe_adapter *adapter)
 static void pch_gbe_configure_rx(struct pch_gbe_adapter *adapter)
 {
         struct pch_gbe_hw *hw = &adapter->hw;
-        u32 rdba, rdlen, rctl, rxdma;
+        u32 rdba, rdlen, rxdma;
 
         pr_debug("dma adr = 0x%08llx  size = 0x%08x\n",
                  (unsigned long long)adapter->rx_ring->dma,
@@ -921,9 +906,7 @@ static void pch_gbe_configure_rx(struct pch_gbe_adapter *adapter)
 
         pch_gbe_mac_force_mac_fc(hw);
 
-        /* Disables Receive MAC */
-        rctl = ioread32(&hw->reg->MAC_RX_EN);
-        iowrite32((rctl & ~PCH_GBE_MRE_MAC_RX_EN), &hw->reg->MAC_RX_EN);
+        pch_gbe_disable_mac_rx(hw);
 
         /* Disables Receive DMA */
         rxdma = ioread32(&hw->reg->DMA_CTRL);
@@ -1316,38 +1299,17 @@ void pch_gbe_update_stats(struct pch_gbe_adapter *adapter)
         spin_unlock_irqrestore(&adapter->stats_lock, flags);
 }
 
-static void pch_gbe_stop_receive(struct pch_gbe_adapter *adapter)
+static void pch_gbe_disable_dma_rx(struct pch_gbe_hw *hw)
 {
-        struct pch_gbe_hw *hw = &adapter->hw;
         u32 rxdma;
-        u16 value;
-        int ret;
 
         /* Disable Receive DMA */
         rxdma = ioread32(&hw->reg->DMA_CTRL);
         rxdma &= ~PCH_GBE_RX_DMA_EN;
         iowrite32(rxdma, &hw->reg->DMA_CTRL);
-        /* Wait Rx DMA BUS is IDLE */
-        ret = pch_gbe_wait_clr_bit_irq(&hw->reg->RX_DMA_ST, PCH_GBE_IDLE_CHECK);
-        if (ret) {
-                /* Disable Bus master */
-                pci_read_config_word(adapter->pdev, PCI_COMMAND, &value);
-                value &= ~PCI_COMMAND_MASTER;
-                pci_write_config_word(adapter->pdev, PCI_COMMAND, value);
-                /* Stop Receive */
-                pch_gbe_mac_reset_rx(hw);
-                /* Enable Bus master */
-                value |= PCI_COMMAND_MASTER;
-                pci_write_config_word(adapter->pdev, PCI_COMMAND, value);
-        } else {
-                /* Stop Receive */
-                pch_gbe_mac_reset_rx(hw);
-        }
-        /* reprogram multicast address register after reset */
-        pch_gbe_set_multi(adapter->netdev);
 }
 
-static void pch_gbe_start_receive(struct pch_gbe_hw *hw)
+static void pch_gbe_enable_dma_rx(struct pch_gbe_hw *hw)
 {
         u32 rxdma;
 
@@ -1355,9 +1317,6 @@ static void pch_gbe_start_receive(struct pch_gbe_hw *hw)
         rxdma = ioread32(&hw->reg->DMA_CTRL);
         rxdma |= PCH_GBE_RX_DMA_EN;
         iowrite32(rxdma, &hw->reg->DMA_CTRL);
-        /* Enables Receive */
-        iowrite32(PCH_GBE_MRE_MAC_RX_EN, &hw->reg->MAC_RX_EN);
-        return;
 }
 
 /**
@@ -1393,7 +1352,7 @@ static irqreturn_t pch_gbe_intr(int irq, void *data)
                         int_en = ioread32(&hw->reg->INT_EN);
                         iowrite32((int_en & ~PCH_GBE_INT_RX_FIFO_ERR),
                                   &hw->reg->INT_EN);
-                        pch_gbe_stop_receive(adapter);
+                        pch_gbe_disable_dma_rx(&adapter->hw);
                         int_st |= ioread32(&hw->reg->INT_ST);
                         int_st = int_st & ioread32(&hw->reg->INT_EN);
                 }
@@ -1971,12 +1930,12 @@ int pch_gbe_up(struct pch_gbe_adapter *adapter)
         struct net_device *netdev = adapter->netdev;
         struct pch_gbe_tx_ring *tx_ring = adapter->tx_ring;
         struct pch_gbe_rx_ring *rx_ring = adapter->rx_ring;
-        int err;
+        int err = -EINVAL;
 
         /* Ensure we have a valid MAC */
         if (!is_valid_ether_addr(adapter->hw.mac.addr)) {
                 pr_err("Error: Invalid MAC address\n");
-                return -EINVAL;
+                goto out;
         }
 
         /* hardware has been reset, we need to reload some things */
@@ -1989,18 +1948,19 @@ int pch_gbe_up(struct pch_gbe_adapter *adapter)
 
         err = pch_gbe_request_irq(adapter);
         if (err) {
-                pr_err("Error: can't bring device up\n");
-                return err;
+                pr_err("Error: can't bring device up - irq request failed\n");
+                goto out;
         }
         err = pch_gbe_alloc_rx_buffers_pool(adapter, rx_ring, rx_ring->count);
         if (err) {
-                pr_err("Error: can't bring device up\n");
-                return err;
+                pr_err("Error: can't bring device up - alloc rx buffers pool failed\n");
+                goto freeirq;
         }
         pch_gbe_alloc_tx_buffers(adapter, tx_ring);
         pch_gbe_alloc_rx_buffers(adapter, rx_ring, rx_ring->count);
         adapter->tx_queue_len = netdev->tx_queue_len;
-        pch_gbe_start_receive(&adapter->hw);
+        pch_gbe_enable_dma_rx(&adapter->hw);
+        pch_gbe_enable_mac_rx(&adapter->hw);
 
         mod_timer(&adapter->watchdog_timer, jiffies);
 
@@ -2009,6 +1969,11 @@ int pch_gbe_up(struct pch_gbe_adapter *adapter)
         netif_start_queue(adapter->netdev);
 
         return 0;
+
+freeirq:
+        pch_gbe_free_irq(adapter);
+out:
+        return err;
 }
 
 /**
@@ -2405,7 +2370,6 @@ static int pch_gbe_napi_poll(struct napi_struct *napi, int budget)
         int work_done = 0;
         bool poll_end_flag = false;
         bool cleaned = false;
-        u32 int_en;
 
         pr_debug("budget : %d\n", budget);
 
@@ -2422,19 +2386,13 @@ static int pch_gbe_napi_poll(struct napi_struct *napi, int budget)
 
         if (poll_end_flag) {
                 napi_complete(napi);
-                if (adapter->rx_stop_flag) {
-                        adapter->rx_stop_flag = false;
-                        pch_gbe_start_receive(&adapter->hw);
-                }
                 pch_gbe_irq_enable(adapter);
-        } else
-                if (adapter->rx_stop_flag) {
-                        adapter->rx_stop_flag = false;
-                        pch_gbe_start_receive(&adapter->hw);
-                        int_en = ioread32(&adapter->hw.reg->INT_EN);
-                        iowrite32((int_en | PCH_GBE_INT_RX_FIFO_ERR),
-                                &adapter->hw.reg->INT_EN);
-                }
+        }
+
+        if (adapter->rx_stop_flag) {
+                adapter->rx_stop_flag = false;
+                pch_gbe_enable_dma_rx(&adapter->hw);
+        }
 
         pr_debug("poll_end_flag : %d  work_done : %d  budget : %d\n",
                  poll_end_flag, work_done, budget);
diff --git a/drivers/net/ethernet/qlogic/qla3xxx.c b/drivers/net/ethernet/qlogic/qla3xxx.c
index df09b1cb742f..6407d0d77e81 100644
--- a/drivers/net/ethernet/qlogic/qla3xxx.c
+++ b/drivers/net/ethernet/qlogic/qla3xxx.c
@@ -2525,6 +2525,13 @@ static int ql_alloc_net_req_rsp_queues(struct ql3_adapter *qdev)
         qdev->req_q_size =
             (u32) (NUM_REQ_Q_ENTRIES * sizeof(struct ob_mac_iocb_req));
 
+        qdev->rsp_q_size = NUM_RSP_Q_ENTRIES * sizeof(struct net_rsp_iocb);
+
+        /* The barrier is required to ensure request and response queue
+         * addr writes to the registers.
+         */
+        wmb();
+
         qdev->req_q_virt_addr =
             pci_alloc_consistent(qdev->pdev,
                                  (size_t) qdev->req_q_size,
@@ -2536,8 +2543,6 @@ static int ql_alloc_net_req_rsp_queues(struct ql3_adapter *qdev)
                 return -ENOMEM;
         }
 
-        qdev->rsp_q_size = NUM_RSP_Q_ENTRIES * sizeof(struct net_rsp_iocb);
-
         qdev->rsp_q_virt_addr =
             pci_alloc_consistent(qdev->pdev,
                                  (size_t) qdev->rsp_q_size,
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 473ce134ca63..24ad17ec7fcd 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -1601,7 +1601,8 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         adapter->netdev  = netdev;
         adapter->pdev    = pdev;
 
-        if (qlcnic_alloc_adapter_resources(adapter))
+        err = qlcnic_alloc_adapter_resources(adapter);
+        if (err)
                 goto err_out_free_netdev;
 
         adapter->dev_rst_time = jiffies;
diff --git a/drivers/net/ethernet/realtek/8139cp.c b/drivers/net/ethernet/realtek/8139cp.c
index 995d0cfc4c06..1c818254b7be 100644
--- a/drivers/net/ethernet/realtek/8139cp.c
+++ b/drivers/net/ethernet/realtek/8139cp.c
@@ -563,7 +563,7 @@ rx_next:
                 if (cpr16(IntrStatus) & cp_rx_intr_mask)
                         goto rx_status_loop;
 
-                napi_gro_flush(napi);
+                napi_gro_flush(napi, false);
                 spin_lock_irqsave(&cp->lock, flags);
                 __napi_complete(napi);
                 cpw16_f(IntrMask, cp_intr_mask);
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index bad8f2eec9b4..c8bfea0524dd 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -2438,6 +2438,7 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
                 rtsu = platform_get_resource(pdev, IORESOURCE_MEM, 1);
                 if (!rtsu) {
                         dev_err(&pdev->dev, "Not found TSU resource\n");
+                        ret = -ENODEV;
                         goto out_release;
                 }
                 mdp->tsu_addr = ioremap(rtsu->start,
diff --git a/drivers/net/ethernet/sfc/ptp.c b/drivers/net/ethernet/sfc/ptp.c
index 5b3dd028ce85..0767043f44a4 100644
--- a/drivers/net/ethernet/sfc/ptp.c
+++ b/drivers/net/ethernet/sfc/ptp.c
@@ -640,8 +640,7 @@ static void efx_ptp_drop_time_expired_events(struct efx_nic *efx)
                         evt = list_entry(cursor, struct efx_ptp_event_rx,
                                          link);
                         if (time_after(jiffies, evt->expiry)) {
-                                list_del(&evt->link);
-                                list_add(&evt->link, &ptp->evt_free_list);
+                                list_move(&evt->link, &ptp->evt_free_list);
                                 netif_warn(efx, hw, efx->net_dev,
                                            "PTP rx event dropped\n");
                         }
@@ -684,8 +683,7 @@ static enum ptp_packet_state efx_ptp_match_rx(struct efx_nic *efx,
 
                         match->state = PTP_PACKET_STATE_MATCHED;
                         rc = PTP_PACKET_STATE_MATCHED;
-                        list_del(&evt->link);
-                        list_add(&evt->link, &ptp->evt_free_list);
+                        list_move(&evt->link, &ptp->evt_free_list);
                         break;
                 }
         }
@@ -820,8 +818,7 @@ static int efx_ptp_stop(struct efx_nic *efx)
         /* Drop any pending receive events */
         spin_lock_bh(&efx->ptp_data->evt_lock);
         list_for_each_safe(cursor, next, &efx->ptp_data->evt_list) {
-                list_del(cursor);
-                list_add(cursor, &efx->ptp_data->evt_free_list);
+                list_move(cursor, &efx->ptp_data->evt_free_list);
         }
         spin_unlock_bh(&efx->ptp_data->evt_lock);
 
diff --git a/drivers/net/ethernet/sis/sis900.c b/drivers/net/ethernet/sis/sis900.c
index 203d9c6ec23a..fb9f6b38511f 100644
--- a/drivers/net/ethernet/sis/sis900.c
+++ b/drivers/net/ethernet/sis/sis900.c
@@ -478,8 +478,10 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 
         /* IO region. */
         ioaddr = pci_iomap(pci_dev, 0, 0);
-        if (!ioaddr)
+        if (!ioaddr) {
+                ret = -ENOMEM;
                 goto err_out_cleardev;
+        }
 
         sis_priv = netdev_priv(net_dev);
         sis_priv->ioaddr = ioaddr;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index e872e1da3137..7d51a65ab099 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -50,7 +50,6 @@ struct stmmac_priv {
         unsigned int dirty_rx;
         struct sk_buff **rx_skbuff;
         dma_addr_t *rx_skbuff_dma;
-        struct sk_buff_head rx_recycle;
 
         struct net_device *dev;
         dma_addr_t dma_rx_phy;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 3be88331d17a..c6cdbc4eb05e 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -747,18 +747,7 @@ static void stmmac_tx(struct stmmac_priv *priv)
                 priv->hw->ring->clean_desc3(p);
 
                 if (likely(skb != NULL)) {
-                        /*
-                         * If there's room in the queue (limit it to size)
-                         * we add this skb back into the pool,
-                         * if it's the right size.
-                         */
-                        if ((skb_queue_len(&priv->rx_recycle) <
-                                priv->dma_rx_size) &&
-                                skb_recycle_check(skb, priv->dma_buf_sz))
-                                __skb_queue_head(&priv->rx_recycle, skb);
-                        else
-                                dev_kfree_skb(skb);
-
+                        dev_kfree_skb(skb);
                         priv->tx_skbuff[entry] = NULL;
                 }
 
@@ -1169,7 +1158,6 @@ static int stmmac_open(struct net_device *dev)
         priv->eee_enabled = stmmac_eee_init(priv);
 
         napi_enable(&priv->napi);
-        skb_queue_head_init(&priv->rx_recycle);
         netif_start_queue(dev);
 
         return 0;
@@ -1222,7 +1210,6 @@ static int stmmac_release(struct net_device *dev)
                 kfree(priv->tm);
 #endif
         napi_disable(&priv->napi);
-        skb_queue_purge(&priv->rx_recycle);
 
         /* Free the IRQ lines */
         free_irq(dev->irq, dev);
@@ -1388,10 +1375,7 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv)
                 if (likely(priv->rx_skbuff[entry] == NULL)) {
                         struct sk_buff *skb;
 
-                        skb = __skb_dequeue(&priv->rx_recycle);
-                        if (skb == NULL)
-                                skb = netdev_alloc_skb_ip_align(priv->dev,
-                                                                bfsize);
+                        skb = netdev_alloc_skb_ip_align(priv->dev, bfsize);
 
                         if (unlikely(skb == NULL))
                                 break;
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 8419bf385e08..275b430aeb75 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -9788,6 +9788,7 @@ static int __devinit niu_pci_init_one(struct pci_dev *pdev,
 
         if (!pci_is_pcie(pdev)) {
                 dev_err(&pdev->dev, "Cannot find PCI Express capability, aborting\n");
+                err = -ENODEV;
                 goto err_out_free_res;
         }
 
diff --git a/drivers/net/ethernet/sun/sungem.c b/drivers/net/ethernet/sun/sungem.c
index 9ae12d0c9632..6c8695ec7cb9 100644
--- a/drivers/net/ethernet/sun/sungem.c
+++ b/drivers/net/ethernet/sun/sungem.c
@@ -2963,7 +2963,8 @@ static int __devinit gem_init_one(struct pci_dev *pdev,
                 goto err_out_iounmap;
         }
 
-        if (gem_get_device_address(gp))
+        err = gem_get_device_address(gp);
+        if (err)
                 goto err_out_free_consistent;
 
         dev->netdev_ops = &gem_netdev_ops;
diff --git a/drivers/net/ethernet/ti/Kconfig b/drivers/net/ethernet/ti/Kconfig
index b26cbda5efa9..2c41894d5472 100644
--- a/drivers/net/ethernet/ti/Kconfig
+++ b/drivers/net/ethernet/ti/Kconfig
@@ -5,7 +5,7 @@
 config NET_VENDOR_TI
         bool "Texas Instruments (TI) devices"
         default y
-        depends on PCI || EISA || AR7 || (ARM && (ARCH_DAVINCI || ARCH_OMAP3))
+        depends on PCI || EISA || AR7 || (ARM && (ARCH_DAVINCI || ARCH_OMAP3 || SOC_AM33XX))
         ---help---
           If you have a network (Ethernet) card belonging to this class, say Y
           and read the Ethernet-HOWTO, available from
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index 4e2a1628484d..4e9810013850 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -1334,11 +1334,11 @@ static int tso_count_edescs(struct sk_buff *skb)
 {
         struct skb_shared_info *sh = skb_shinfo(skb);
         unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
-        unsigned int data_len = skb->data_len + skb->hdr_len - sh_len;
+        unsigned int data_len = skb->len - sh_len;
         unsigned int p_len = sh->gso_size;
         long f_id = -1;    /* id of the current fragment */
-        long f_size = skb->hdr_len;  /* size of the current fragment */
-        long f_used = sh_len;  /* bytes used from the current fragment */
+        long f_size = skb_headlen(skb) - sh_len;  /* current fragment size */
+        long f_used = 0;  /* bytes used from the current fragment */
         long n;            /* size of the current piece of payload */
         int num_edescs = 0;
         int segment;
@@ -1353,7 +1353,7 @@ static int tso_count_edescs(struct sk_buff *skb)
                         /* Advance as needed. */
                         while (f_used >= f_size) {
                                 f_id++;
-                                f_size = sh->frags[f_id].size;
+                                f_size = skb_frag_size(&sh->frags[f_id]);
                                 f_used = 0;
                         }
 
@@ -1384,13 +1384,13 @@ static void tso_headers_prepare(struct sk_buff *skb, unsigned char *headers,
         struct iphdr *ih;
         struct tcphdr *th;
         unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
-        unsigned int data_len = skb->data_len + skb->hdr_len - sh_len;
+        unsigned int data_len = skb->len - sh_len;
         unsigned char *data = skb->data;
         unsigned int ih_off, th_off, p_len;
         unsigned int isum_seed, tsum_seed, id, seq;
         long f_id = -1;    /* id of the current fragment */
-        long f_size = skb->hdr_len;  /* size of the current fragment */
-        long f_used = sh_len;  /* bytes used from the current fragment */
+        long f_size = skb_headlen(skb) - sh_len;  /* current fragment size */
+        long f_used = 0;  /* bytes used from the current fragment */
         long n;            /* size of the current piece of payload */
         int segment;
 
@@ -1405,7 +1405,7 @@ static void tso_headers_prepare(struct sk_buff *skb, unsigned char *headers,
         isum_seed = ((0xFFFF - ih->check) +
                      (0xFFFF - ih->tot_len) +
                      (0xFFFF - ih->id));
-        tsum_seed = th->check + (0xFFFF ^ htons(sh_len + data_len));
+        tsum_seed = th->check + (0xFFFF ^ htons(skb->len));
         id = ntohs(ih->id);
         seq = ntohl(th->seq);
 
@@ -1444,7 +1444,7 @@ static void tso_headers_prepare(struct sk_buff *skb, unsigned char *headers,
                         /* Advance as needed. */
                         while (f_used >= f_size) {
                                 f_id++;
-                                f_size = sh->frags[f_id].size;
+                                f_size = skb_frag_size(&sh->frags[f_id]);
                                 f_used = 0;
                         }
 
@@ -1478,14 +1478,14 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
         struct tile_net_priv *priv = netdev_priv(dev);
         struct skb_shared_info *sh = skb_shinfo(skb);
         unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
-        unsigned int data_len = skb->data_len + skb->hdr_len - sh_len;
+        unsigned int data_len = skb->len - sh_len;
         unsigned int p_len = sh->gso_size;
         gxio_mpipe_edesc_t edesc_head = { { 0 } };
         gxio_mpipe_edesc_t edesc_body = { { 0 } };
         long f_id = -1;    /* id of the current fragment */
-        long f_size = skb->hdr_len;  /* size of the current fragment */
-        long f_used = sh_len;  /* bytes used from the current fragment */
-        void *f_data = skb->data;
+        long f_size = skb_headlen(skb) - sh_len;  /* current fragment size */
+        long f_used = 0;  /* bytes used from the current fragment */
+        void *f_data = skb->data + sh_len;
         long n;            /* size of the current piece of payload */
         unsigned long tx_packets = 0, tx_bytes = 0;
         unsigned int csum_start;
@@ -1516,15 +1516,18 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
 
                 /* Egress the payload. */
                 while (p_used < p_len) {
+                        void *va;
 
                         /* Advance as needed. */
                         while (f_used >= f_size) {
                                 f_id++;
-                                f_size = sh->frags[f_id].size;
-                                f_used = 0;
+                                f_size = skb_frag_size(&sh->frags[f_id]);
                                 f_data = tile_net_frag_buf(&sh->frags[f_id]);
+                                f_used = 0;
                         }
 
+                        va = f_data + f_used;
+
                         /* Use bytes from the current fragment. */
                         n = p_len - p_used;
                         if (n > f_size - f_used)
@@ -1533,7 +1536,7 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
                         p_used += n;
 
                         /* Egress a piece of the payload. */
-                        edesc_body.va = va_to_tile_io_addr(f_data) + f_used;
+                        edesc_body.va = va_to_tile_io_addr(va);
                         edesc_body.xfer_size = n;
                         edesc_body.bound = !(p_used < p_len);
                         gxio_mpipe_equeue_put_at(equeue, edesc_body, slot);
diff --git a/drivers/net/irda/irtty-sir.c b/drivers/net/irda/irtty-sir.c
index 30087ca23a0f..6e4d4b62c9a8 100644
--- a/drivers/net/irda/irtty-sir.c
+++ b/drivers/net/irda/irtty-sir.c
@@ -459,8 +459,10 @@ static int irtty_open(struct tty_struct *tty)
 
         /* allocate private device info block */
         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-        if (!priv)
+        if (!priv) {
+                ret = -ENOMEM;
                 goto out_put;
+        }
 
         priv->magic = IRTTY_MAGIC;
         priv->tty = tty;
diff --git a/drivers/net/irda/mcs7780.c b/drivers/net/irda/mcs7780.c
index 1a00b5990cb8..f07c340990da 100644
--- a/drivers/net/irda/mcs7780.c
+++ b/drivers/net/irda/mcs7780.c
@@ -920,8 +920,10 @@ static int mcs_probe(struct usb_interface *intf,
 
         ndev->netdev_ops = &mcs_netdev_ops;
 
-        if (!intf->cur_altsetting)
+        if (!intf->cur_altsetting) {
+                ret = -ENOMEM;
                 goto error2;
+        }
 
         ret = mcs_find_endpoints(mcs, intf->cur_altsetting->endpoint,
                                  intf->cur_altsetting->desc.bNumEndpoints);
diff --git a/drivers/net/irda/pxaficp_ir.c b/drivers/net/irda/pxaficp_ir.c
index 002a442bf73f..858de05bdb7d 100644
--- a/drivers/net/irda/pxaficp_ir.c
+++ b/drivers/net/irda/pxaficp_ir.c
@@ -846,8 +846,10 @@ static int pxa_irda_probe(struct platform_device *pdev)
                 goto err_mem_2;
 
         dev = alloc_irdadev(sizeof(struct pxa_irda));
-        if (!dev)
+        if (!dev) {
+                err = -ENOMEM;
                 goto err_mem_3;
+        }
 
         SET_NETDEV_DEV(dev, &pdev->dev);
         si = netdev_priv(dev);
diff --git a/drivers/net/irda/sa1100_ir.c b/drivers/net/irda/sa1100_ir.c
index e25067552b20..42fde9ed23e1 100644
--- a/drivers/net/irda/sa1100_ir.c
+++ b/drivers/net/irda/sa1100_ir.c
@@ -940,8 +940,10 @@ static int sa1100_irda_probe(struct platform_device *pdev)
                 goto err_mem_3;
 
         dev = alloc_irdadev(sizeof(struct sa1100_irda));
-        if (!dev)
+        if (!dev) {
+                err = -ENOMEM;
                 goto err_mem_4;
+        }
 
         SET_NETDEV_DEV(dev, &pdev->dev);
 
diff --git a/drivers/net/irda/sh_irda.c b/drivers/net/irda/sh_irda.c
index eb315b8d07a3..4b746d9bd8e7 100644
--- a/drivers/net/irda/sh_irda.c
+++ b/drivers/net/irda/sh_irda.c
@@ -808,8 +808,8 @@ static int __devinit sh_irda_probe(struct platform_device *pdev)
                 goto err_mem_4;
 
         platform_set_drvdata(pdev, ndev);
-
-        if (request_irq(irq, sh_irda_irq, IRQF_DISABLED, "sh_irda", self)) {
+        err = request_irq(irq, sh_irda_irq, IRQF_DISABLED, "sh_irda", self);
+        if (err) {
                 dev_warn(&pdev->dev, "Unable to attach sh_irda interrupt\n");
                 goto err_mem_4;
         }
diff --git a/drivers/net/irda/sh_sir.c b/drivers/net/irda/sh_sir.c
index 795109425568..624ac1939e85 100644
--- a/drivers/net/irda/sh_sir.c
+++ b/drivers/net/irda/sh_sir.c
@@ -741,6 +741,7 @@ static int __devinit sh_sir_probe(struct platform_device *pdev)
         self->clk = clk_get(&pdev->dev, clk_name);
         if (IS_ERR(self->clk)) {
                 dev_err(&pdev->dev, "cannot get clock \"%s\"\n", clk_name);
+                err = -ENODEV;
                 goto err_mem_3;
         }
 
@@ -760,8 +761,8 @@ static int __devinit sh_sir_probe(struct platform_device *pdev)
                 goto err_mem_4;
 
         platform_set_drvdata(pdev, ndev);
-
-        if (request_irq(irq, sh_sir_irq, IRQF_DISABLED, "sh_sir", self)) {
+        err = request_irq(irq, sh_sir_irq, IRQF_DISABLED, "sh_sir", self);
+        if (err) {
                 dev_warn(&pdev->dev, "Unable to attach sh_sir interrupt\n");
                 goto err_mem_4;
         }
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 983bbf4d5ef6..961f0b293913 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -15,6 +15,11 @@ if PHYLIB
 
 comment "MII PHY device drivers"
 
+config AT803X_PHY
+        tristate "Drivers for Atheros AT803X PHYs"
+        ---help---
+          Currently supports the AT8030 and AT8035 model
+
 config AMD_PHY
         tristate "Drivers for the AMD PHYs"
         ---help---
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index 426674debae4..9645e389a58d 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_STE10XP)		+= ste10Xp.o
 obj-$(CONFIG_MICREL_PHY)        += micrel.o
 obj-$(CONFIG_MDIO_OCTEON)       += mdio-octeon.o
 obj-$(CONFIG_MICREL_KS8995MA)   += spi_ks8995.o
+obj-$(CONFIG_AT803X_PHY)        += at803x.o
 obj-$(CONFIG_AMD_PHY)           += amd.o
 obj-$(CONFIG_MDIO_BUS_MUX)      += mdio-mux.o
 obj-$(CONFIG_MDIO_BUS_MUX_GPIO) += mdio-mux-gpio.o
diff --git a/drivers/net/phy/at803x.c b/drivers/net/phy/at803x.c
new file mode 100644
index 000000000000..45cbc10de01c
--- /dev/null
+++ b/drivers/net/phy/at803x.c
@@ -0,0 +1,176 @@
+/*
+ * drivers/net/phy/at803x.c
+ *
+ * Driver for Atheros 803x PHY
+ *
+ * Author: Matus Ujhelyi <ujhelyi.m@gmail.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/phy.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+
+#define AT803X_INTR_ENABLE                      0x12
+#define AT803X_INTR_STATUS                      0x13
+#define AT803X_WOL_ENABLE                       0x01
+#define AT803X_DEVICE_ADDR                      0x03
+#define AT803X_LOC_MAC_ADDR_0_15_OFFSET         0x804C
+#define AT803X_LOC_MAC_ADDR_16_31_OFFSET        0x804B
+#define AT803X_LOC_MAC_ADDR_32_47_OFFSET        0x804A
+#define AT803X_MMD_ACCESS_CONTROL               0x0D
+#define AT803X_MMD_ACCESS_CONTROL_DATA          0x0E
+#define AT803X_FUNC_DATA                        0x4003
+
+MODULE_DESCRIPTION("Atheros 803x PHY driver");
+MODULE_AUTHOR("Matus Ujhelyi");
+MODULE_LICENSE("GPL");
+
+static void at803x_set_wol_mac_addr(struct phy_device *phydev)
+{
+        struct net_device *ndev = phydev->attached_dev;
+        const u8 *mac;
+        unsigned int i, offsets[] = {
+                AT803X_LOC_MAC_ADDR_32_47_OFFSET,
+                AT803X_LOC_MAC_ADDR_16_31_OFFSET,
+                AT803X_LOC_MAC_ADDR_0_15_OFFSET,
+        };
+
+        if (!ndev)
+                return;
+
+        mac = (const u8 *) ndev->dev_addr;
+
+        if (!is_valid_ether_addr(mac))
+                return;
+
+        for (i = 0; i < 3; i++) {
+                phy_write(phydev, AT803X_MMD_ACCESS_CONTROL,
+                                  AT803X_DEVICE_ADDR);
+                phy_write(phydev, AT803X_MMD_ACCESS_CONTROL_DATA,
+                                  offsets[i]);
+                phy_write(phydev, AT803X_MMD_ACCESS_CONTROL,
+                                  AT803X_FUNC_DATA);
+                phy_write(phydev, AT803X_MMD_ACCESS_CONTROL_DATA,
+                                  mac[(i * 2) + 1] | (mac[(i * 2)] << 8));
+        }
+}
+
+static int at803x_config_init(struct phy_device *phydev)
+{
+        int val;
+        u32 features;
+        int status;
+
+        features = SUPPORTED_TP | SUPPORTED_MII | SUPPORTED_AUI |
+                   SUPPORTED_FIBRE | SUPPORTED_BNC;
+
+        val = phy_read(phydev, MII_BMSR);
+        if (val < 0)
+                return val;
+
+        if (val & BMSR_ANEGCAPABLE)
+                features |= SUPPORTED_Autoneg;
+        if (val & BMSR_100FULL)
+                features |= SUPPORTED_100baseT_Full;
+        if (val & BMSR_100HALF)
+                features |= SUPPORTED_100baseT_Half;
+        if (val & BMSR_10FULL)
+                features |= SUPPORTED_10baseT_Full;
+        if (val & BMSR_10HALF)
+                features |= SUPPORTED_10baseT_Half;
+
+        if (val & BMSR_ESTATEN) {
+                val = phy_read(phydev, MII_ESTATUS);
+                if (val < 0)
+                        return val;
+
+                if (val & ESTATUS_1000_TFULL)
+                        features |= SUPPORTED_1000baseT_Full;
+                if (val & ESTATUS_1000_THALF)
+                        features |= SUPPORTED_1000baseT_Half;
+        }
+
+        phydev->supported = features;
+        phydev->advertising = features;
+
+        /* enable WOL */
+        at803x_set_wol_mac_addr(phydev);
+        status = phy_write(phydev, AT803X_INTR_ENABLE, AT803X_WOL_ENABLE);
+        status = phy_read(phydev, AT803X_INTR_STATUS);
+
+        return 0;
+}
+
+/* ATHEROS 8035 */
+static struct phy_driver at8035_driver = {
+        .phy_id         = 0x004dd072,
+        .name           = "Atheros 8035 ethernet",
+        .phy_id_mask    = 0xffffffef,
+        .config_init    = at803x_config_init,
+        .features       = PHY_GBIT_FEATURES,
+        .flags          = PHY_HAS_INTERRUPT,
+        .config_aneg    = &genphy_config_aneg,
+        .read_status    = &genphy_read_status,
+        .driver         = {
+                .owner = THIS_MODULE,
+        },
+};
+
+/* ATHEROS 8030 */
+static struct phy_driver at8030_driver = {
+        .phy_id         = 0x004dd076,
+        .name           = "Atheros 8030 ethernet",
+        .phy_id_mask    = 0xffffffef,
+        .config_init    = at803x_config_init,
+        .features       = PHY_GBIT_FEATURES,
+        .flags          = PHY_HAS_INTERRUPT,
+        .config_aneg    = &genphy_config_aneg,
+        .read_status    = &genphy_read_status,
+        .driver         = {
+                .owner = THIS_MODULE,
+        },
+};
+
+static int __init atheros_init(void)
+{
+        int ret;
+
+        ret = phy_driver_register(&at8035_driver);
+        if (ret)
+                goto err1;
+
+        ret = phy_driver_register(&at8030_driver);
+        if (ret)
+                goto err2;
+
+        return 0;
+
+err2:
+        phy_driver_unregister(&at8035_driver);
+err1:
+        return ret;
+}
+
+static void __exit atheros_exit(void)
+{
+        phy_driver_unregister(&at8035_driver);
+        phy_driver_unregister(&at8030_driver);
+}
+
+module_init(atheros_init);
+module_exit(atheros_exit);
+
+static struct mdio_device_id __maybe_unused atheros_tbl[] = {
+        { 0x004dd076, 0xffffffef },
+        { 0x004dd072, 0xffffffef },
+        { }
+};
+
+MODULE_DEVICE_TABLE(mdio, atheros_tbl);
diff --git a/drivers/net/phy/mdio-bitbang.c b/drivers/net/phy/mdio-bitbang.c
index daec9b05d168..6428fcbbdd4b 100644
--- a/drivers/net/phy/mdio-bitbang.c
+++ b/drivers/net/phy/mdio-bitbang.c
@@ -234,6 +234,7 @@ void free_mdio_bitbang(struct mii_bus *bus)
         struct mdiobb_ctrl *ctrl = bus->priv;
 
         module_put(ctrl->ops->owner);
+        mdiobus_unregister(bus);
         mdiobus_free(bus);
 }
 EXPORT_SYMBOL(free_mdio_bitbang);
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 170eb411ab5d..c1ef3000ea60 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/of_device.h>
+#include <linux/of_mdio.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
diff --git a/drivers/net/usb/cdc_eem.c b/drivers/net/usb/cdc_eem.c
index 434d5af8e6fb..c81e278629ff 100644
--- a/drivers/net/usb/cdc_eem.c
+++ b/drivers/net/usb/cdc_eem.c
@@ -244,8 +244,12 @@ static int eem_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
                          *  - suspend: peripheral ready to suspend
                          *  - response: suggest N millisec polling
                          *  - response complete: suggest N sec polling
+                         *
+                         * Suspend is reported and maybe heeded.
                          */
                         case 2:         /* Suspend hint */
+                                usbnet_device_suggests_idle(dev);
+                                continue;
                         case 3:         /* Response hint */
                         case 4:         /* Response complete hint */
                                 continue;
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index a03de7197049..d0129827602b 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -592,6 +592,32 @@ static const struct usb_device_id	products [] = {
         .driver_info            = 0,
 },
 
+/* Novatel USB551L and MC551 - handled by qmi_wwan */
+{
+        .match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+                 | USB_DEVICE_ID_MATCH_PRODUCT
+                 | USB_DEVICE_ID_MATCH_INT_INFO,
+        .idVendor               = NOVATEL_VENDOR_ID,
+        .idProduct              = 0xB001,
+        .bInterfaceClass        = USB_CLASS_COMM,
+        .bInterfaceSubClass     = USB_CDC_SUBCLASS_ETHERNET,
+        .bInterfaceProtocol     = USB_CDC_PROTO_NONE,
+        .driver_info = 0,
+},
+
+/* Novatel E362 - handled by qmi_wwan */
+{
+        .match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+                 | USB_DEVICE_ID_MATCH_PRODUCT
+                 | USB_DEVICE_ID_MATCH_INT_INFO,
+        .idVendor               = NOVATEL_VENDOR_ID,
+        .idProduct              = 0x9010,
+        .bInterfaceClass        = USB_CLASS_COMM,
+        .bInterfaceSubClass     = USB_CDC_SUBCLASS_ETHERNET,
+        .bInterfaceProtocol     = USB_CDC_PROTO_NONE,
+        .driver_info = 0,
+},
+
 /*
  * WHITELIST!!!
  *
@@ -604,21 +630,6 @@ static const struct usb_device_id	products [] = {
  * because of bugs/quirks in a given product (like Zaurus, above).
  */
 {
-        /* Novatel USB551L */
-        /* This match must come *before* the generic CDC-ETHER match so that
-         * we get FLAG_WWAN set on the device, since it's descriptors are
-         * generic CDC-ETHER.
-         */
-        .match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
-                 | USB_DEVICE_ID_MATCH_PRODUCT
-                 | USB_DEVICE_ID_MATCH_INT_INFO,
-        .idVendor               = NOVATEL_VENDOR_ID,
-        .idProduct              = 0xB001,
-        .bInterfaceClass        = USB_CLASS_COMM,
-        .bInterfaceSubClass     = USB_CDC_SUBCLASS_ETHERNET,
-        .bInterfaceProtocol     = USB_CDC_PROTO_NONE,
-        .driver_info = (unsigned long)&wwan_info,
-}, {
         /* ZTE (Vodafone) K3805-Z */
         .match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
                  | USB_DEVICE_ID_MATCH_PRODUCT
diff --git a/drivers/net/usb/ipheth.c b/drivers/net/usb/ipheth.c
index a28a983d465e..534d8becbbdc 100644
--- a/drivers/net/usb/ipheth.c
+++ b/drivers/net/usb/ipheth.c
@@ -62,6 +62,7 @@
 #define USB_PRODUCT_IPAD 0x129a
 #define USB_PRODUCT_IPHONE_4_VZW 0x129c
 #define USB_PRODUCT_IPHONE_4S   0x12a0
+#define USB_PRODUCT_IPHONE_5    0x12a8
 
 #define IPHETH_USBINTF_CLASS    255
 #define IPHETH_USBINTF_SUBCLASS 253
@@ -113,6 +114,10 @@ static struct usb_device_id ipheth_table[] = {
                 USB_VENDOR_APPLE, USB_PRODUCT_IPHONE_4S,
                 IPHETH_USBINTF_CLASS, IPHETH_USBINTF_SUBCLASS,
                 IPHETH_USBINTF_PROTO) },
+        { USB_DEVICE_AND_INTERFACE_INFO(
+                USB_VENDOR_APPLE, USB_PRODUCT_IPHONE_5,
+                IPHETH_USBINTF_CLASS, IPHETH_USBINTF_SUBCLASS,
+                IPHETH_USBINTF_PROTO) },
         { }
 };
 MODULE_DEVICE_TABLE(usb, ipheth_table);
diff --git a/drivers/net/usb/kaweth.c b/drivers/net/usb/kaweth.c
index c75e11e1b385..afb117c16d2d 100644
--- a/drivers/net/usb/kaweth.c
+++ b/drivers/net/usb/kaweth.c
@@ -424,7 +424,7 @@ static int kaweth_download_firmware(struct kaweth_device *kaweth,
 
         netdev_dbg(kaweth->net,
                    "Downloading firmware at %p to kaweth device at %p\n",
-                   fw->data, kaweth);
+                   kaweth->firmware_buf, kaweth);
         netdev_dbg(kaweth->net, "Firmware length: %d\n", data_len);
 
         return kaweth_control(kaweth,
diff --git a/drivers/net/usb/mcs7830.c b/drivers/net/usb/mcs7830.c
index 03c2d8d653df..cc7e72010ac3 100644
--- a/drivers/net/usb/mcs7830.c
+++ b/drivers/net/usb/mcs7830.c
@@ -117,6 +117,7 @@ enum {
 struct mcs7830_data {
         u8 multi_filter[8];
         u8 config;
+        u8 link_counter;
 };
 
 static const char driver_name[] = "MOSCHIP usb-ethernet driver";
@@ -632,20 +633,31 @@ static int mcs7830_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 static void mcs7830_status(struct usbnet *dev, struct urb *urb)
 {
         u8 *buf = urb->transfer_buffer;
-        bool link;
+        bool link, link_changed;
+        struct mcs7830_data *data = mcs7830_get_data(dev);
 
         if (urb->actual_length < 16)
                 return;
 
         link = !(buf[1] & 0x20);
-        if (netif_carrier_ok(dev->net) != link) {
-                if (link) {
-                        netif_carrier_on(dev->net);
-                        usbnet_defer_kevent(dev, EVENT_LINK_RESET);
-                } else
-                        netif_carrier_off(dev->net);
-                netdev_dbg(dev->net, "Link Status is: %d\n", link);
-        }
+        link_changed = netif_carrier_ok(dev->net) != link;
+        if (link_changed) {
+                data->link_counter++;
+                /*
+                   track link state 20 times to guard against erroneous
+                   link state changes reported sometimes by the chip
+                 */
+                if (data->link_counter > 20) {
+                        data->link_counter = 0;
+                        if (link) {
+                                netif_carrier_on(dev->net);
+                                usbnet_defer_kevent(dev, EVENT_LINK_RESET);
+                        } else
+                                netif_carrier_off(dev->net);
+                        netdev_dbg(dev->net, "Link Status is: %d\n", link);
+                }
+        } else
+                data->link_counter = 0;
 }
 
 static const struct driver_info moschip_info = {
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 6883c371c59f..3b566fa0f8e6 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -369,18 +369,73 @@ static const struct usb_device_id products[] = {
                 USB_VENDOR_AND_INTERFACE_INFO(0x106c, USB_CLASS_VENDOR_SPEC, 0xf1, 0xff),
                 .driver_info        = (unsigned long)&qmi_wwan_info,
         },
+        {       /* Novatel USB551L and MC551 */
+                USB_DEVICE_AND_INTERFACE_INFO(0x1410, 0xb001,
+                                              USB_CLASS_COMM,
+                                              USB_CDC_SUBCLASS_ETHERNET,
+                                              USB_CDC_PROTO_NONE),
+                .driver_info        = (unsigned long)&qmi_wwan_info,
+        },
+        {       /* Novatel E362 */
+                USB_DEVICE_AND_INTERFACE_INFO(0x1410, 0x9010,
+                                              USB_CLASS_COMM,
+                                              USB_CDC_SUBCLASS_ETHERNET,
+                                              USB_CDC_PROTO_NONE),
+                .driver_info        = (unsigned long)&qmi_wwan_info,
+        },
 
         /* 3. Combined interface devices matching on interface number */
+        {QMI_FIXED_INTF(0x19d2, 0x0002, 1)},
+        {QMI_FIXED_INTF(0x19d2, 0x0012, 1)},
+        {QMI_FIXED_INTF(0x19d2, 0x0017, 3)},
+        {QMI_FIXED_INTF(0x19d2, 0x0021, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x0025, 1)},
+        {QMI_FIXED_INTF(0x19d2, 0x0031, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x0042, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x0049, 5)},
+        {QMI_FIXED_INTF(0x19d2, 0x0052, 4)},
         {QMI_FIXED_INTF(0x19d2, 0x0055, 1)},    /* ZTE (Vodafone) K3520-Z */
+        {QMI_FIXED_INTF(0x19d2, 0x0058, 4)},
         {QMI_FIXED_INTF(0x19d2, 0x0063, 4)},    /* ZTE (Vodafone) K3565-Z */
         {QMI_FIXED_INTF(0x19d2, 0x0104, 4)},    /* ZTE (Vodafone) K4505-Z */
+        {QMI_FIXED_INTF(0x19d2, 0x0113, 5)},
+        {QMI_FIXED_INTF(0x19d2, 0x0118, 5)},
+        {QMI_FIXED_INTF(0x19d2, 0x0121, 5)},
+        {QMI_FIXED_INTF(0x19d2, 0x0123, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x0124, 5)},
+        {QMI_FIXED_INTF(0x19d2, 0x0125, 6)},
+        {QMI_FIXED_INTF(0x19d2, 0x0126, 5)},
+        {QMI_FIXED_INTF(0x19d2, 0x0130, 1)},
+        {QMI_FIXED_INTF(0x19d2, 0x0133, 3)},
+        {QMI_FIXED_INTF(0x19d2, 0x0141, 5)},
         {QMI_FIXED_INTF(0x19d2, 0x0157, 5)},    /* ZTE MF683 */
+        {QMI_FIXED_INTF(0x19d2, 0x0158, 3)},
         {QMI_FIXED_INTF(0x19d2, 0x0167, 4)},    /* ZTE MF820D */
+        {QMI_FIXED_INTF(0x19d2, 0x0168, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x0176, 3)},
+        {QMI_FIXED_INTF(0x19d2, 0x0178, 3)},
+        {QMI_FIXED_INTF(0x19d2, 0x0191, 4)},    /* ZTE EuFi890 */
+        {QMI_FIXED_INTF(0x19d2, 0x0199, 1)},    /* ZTE MF820S */
+        {QMI_FIXED_INTF(0x19d2, 0x0200, 1)},
+        {QMI_FIXED_INTF(0x19d2, 0x0257, 3)},    /* ZTE MF821 */
         {QMI_FIXED_INTF(0x19d2, 0x0326, 4)},    /* ZTE MF821D */
         {QMI_FIXED_INTF(0x19d2, 0x1008, 4)},    /* ZTE (Vodafone) K3570-Z */
         {QMI_FIXED_INTF(0x19d2, 0x1010, 4)},    /* ZTE (Vodafone) K3571-Z */
+        {QMI_FIXED_INTF(0x19d2, 0x1012, 4)},
         {QMI_FIXED_INTF(0x19d2, 0x1018, 3)},    /* ZTE (Vodafone) K5006-Z */
+        {QMI_FIXED_INTF(0x19d2, 0x1021, 2)},
+        {QMI_FIXED_INTF(0x19d2, 0x1245, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x1247, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x1252, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x1254, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x1255, 3)},
+        {QMI_FIXED_INTF(0x19d2, 0x1255, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x1256, 4)},
+        {QMI_FIXED_INTF(0x19d2, 0x1401, 2)},
         {QMI_FIXED_INTF(0x19d2, 0x1402, 2)},    /* ZTE MF60 */
+        {QMI_FIXED_INTF(0x19d2, 0x1424, 2)},
+        {QMI_FIXED_INTF(0x19d2, 0x1425, 2)},
+        {QMI_FIXED_INTF(0x19d2, 0x1426, 2)},    /* ZTE MF91 */
         {QMI_FIXED_INTF(0x19d2, 0x2002, 4)},    /* ZTE (Vodafone) K3765-Z */
         {QMI_FIXED_INTF(0x0f3d, 0x68a2, 8)},    /* Sierra Wireless MC7700 */
         {QMI_FIXED_INTF(0x114f, 0x68a2, 8)},    /* Sierra Wireless MC7750 */
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index fc9f578a1e25..cb04f900cc46 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -1158,6 +1158,7 @@ netdev_tx_t usbnet_start_xmit (struct sk_buff *skb,
                 usb_anchor_urb(urb, &dev->deferred);
                 /* no use to process more packets */
                 netif_stop_queue(net);
+                usb_put_urb(urb);
                 spin_unlock_irqrestore(&dev->txq.lock, flags);
                 netdev_dbg(dev->net, "Delaying transmission for resumption\n");
                 goto deferred;
@@ -1310,6 +1311,8 @@ void usbnet_disconnect (struct usb_interface *intf)
 
         cancel_work_sync(&dev->kevent);
 
+        usb_scuttle_anchored_urbs(&dev->deferred);
+
         if (dev->driver_info->unbind)
                 dev->driver_info->unbind (dev, intf);
 
@@ -1588,10 +1591,27 @@ int usbnet_resume (struct usb_interface *intf)
                         tasklet_schedule (&dev->bh);
                 }
         }
+
+        if (test_and_clear_bit(EVENT_DEVICE_REPORT_IDLE, &dev->flags))
+                usb_autopm_get_interface_no_resume(intf);
+
         return 0;
 }
 EXPORT_SYMBOL_GPL(usbnet_resume);
 
+/*
+ * Either a subdriver implements manage_power, then it is assumed to always
+ * be ready to be suspended or it reports the readiness to be suspended
+ * explicitly
+ */
+void usbnet_device_suggests_idle(struct usbnet *dev)
+{
+        if (!test_and_set_bit(EVENT_DEVICE_REPORT_IDLE, &dev->flags)) {
+                dev->intf->needs_remote_wakeup = 1;
+                usb_autopm_put_interface_async(dev->intf);
+        }
+}
+EXPORT_SYMBOL(usbnet_device_suggests_idle);
 
 /*-------------------------------------------------------------------------*/
 
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index ce9d4f2c9776..0ae1bcc6da73 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -744,28 +744,43 @@ vmxnet3_map_pkt(struct sk_buff *skb, struct vmxnet3_tx_ctx *ctx,
 
         for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
                 const struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+                u32 buf_size;
 
-                tbi = tq->buf_info + tq->tx_ring.next2fill;
-                tbi->map_type = VMXNET3_MAP_PAGE;
-                tbi->dma_addr = skb_frag_dma_map(&adapter->pdev->dev, frag,
-                                                 0, skb_frag_size(frag),
-                                                 DMA_TO_DEVICE);
+                buf_offset = 0;
+                len = skb_frag_size(frag);
+                while (len) {
+                        tbi = tq->buf_info + tq->tx_ring.next2fill;
+                        if (len < VMXNET3_MAX_TX_BUF_SIZE) {
+                                buf_size = len;
+                                dw2 |= len;
+                        } else {
+                                buf_size = VMXNET3_MAX_TX_BUF_SIZE;
+                                /* spec says that for TxDesc.len, 0 == 2^14 */
+                        }
+                        tbi->map_type = VMXNET3_MAP_PAGE;
+                        tbi->dma_addr = skb_frag_dma_map(&adapter->pdev->dev, frag,
+                                                         buf_offset, buf_size,
+                                                         DMA_TO_DEVICE);
 
-                tbi->len = skb_frag_size(frag);
+                        tbi->len = buf_size;
 
-                gdesc = tq->tx_ring.base + tq->tx_ring.next2fill;
-                BUG_ON(gdesc->txd.gen == tq->tx_ring.gen);
+                        gdesc = tq->tx_ring.base + tq->tx_ring.next2fill;
+                        BUG_ON(gdesc->txd.gen == tq->tx_ring.gen);
 
-                gdesc->txd.addr = cpu_to_le64(tbi->dma_addr);
-                gdesc->dword[2] = cpu_to_le32(dw2 | skb_frag_size(frag));
-                gdesc->dword[3] = 0;
+                        gdesc->txd.addr = cpu_to_le64(tbi->dma_addr);
+                        gdesc->dword[2] = cpu_to_le32(dw2);
+                        gdesc->dword[3] = 0;
 
-                dev_dbg(&adapter->netdev->dev,
-                        "txd[%u]: 0x%llu %u %u\n",
-                        tq->tx_ring.next2fill, le64_to_cpu(gdesc->txd.addr),
-                        le32_to_cpu(gdesc->dword[2]), gdesc->dword[3]);
-                vmxnet3_cmd_ring_adv_next2fill(&tq->tx_ring);
-                dw2 = tq->tx_ring.gen << VMXNET3_TXD_GEN_SHIFT;
+                        dev_dbg(&adapter->netdev->dev,
+                                "txd[%u]: 0x%llu %u %u\n",
+                                tq->tx_ring.next2fill, le64_to_cpu(gdesc->txd.addr),
+                                le32_to_cpu(gdesc->dword[2]), gdesc->dword[3]);
+                        vmxnet3_cmd_ring_adv_next2fill(&tq->tx_ring);
+                        dw2 = tq->tx_ring.gen << VMXNET3_TXD_GEN_SHIFT;
+
+                        len -= buf_size;
+                        buf_offset += buf_size;
+                }
         }
 
         ctx->eop_txd = gdesc;
@@ -886,6 +901,18 @@ vmxnet3_prepare_tso(struct sk_buff *skb,
         }
 }
 
+static int txd_estimate(const struct sk_buff *skb)
+{
+        int count = VMXNET3_TXD_NEEDED(skb_headlen(skb)) + 1;
+        int i;
+
+        for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+                const struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+
+                count += VMXNET3_TXD_NEEDED(skb_frag_size(frag));
+        }
+        return count;
+}
 
 /*
  * Transmits a pkt thru a given tq
@@ -914,9 +941,7 @@ vmxnet3_tq_xmit(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
         union Vmxnet3_GenericDesc tempTxDesc;
 #endif
 
-        /* conservatively estimate # of descriptors to use */
-        count = VMXNET3_TXD_NEEDED(skb_headlen(skb)) +
-                skb_shinfo(skb)->nr_frags + 1;
+        count = txd_estimate(skb);
 
         ctx.ipv4 = (vlan_get_protocol(skb) == cpu_to_be16(ETH_P_IP));
 
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 51de9edb55f5..7b4adde93c01 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -28,7 +28,6 @@
 #include <linux/igmp.h>
 #include <linux/etherdevice.h>
 #include <linux/if_ether.h>
-#include <linux/version.h>
 #include <linux/hash.h>
 #include <net/ip.h>
 #include <net/icmp.h>
@@ -107,6 +106,8 @@ struct vxlan_dev {
         __be32            gaddr;        /* multicast group */
         __be32            saddr;        /* source address */
         unsigned int      link;         /* link to multicast over */
+        __u16             port_min;     /* source port range */
+        __u16             port_max;
         __u8              tos;          /* TOS override */
         __u8              ttl;
         bool              learn;
@@ -229,9 +230,9 @@ static u32 eth_hash(const unsigned char *addr)
 
         /* only want 6 bytes */
 #ifdef __BIG_ENDIAN
-        value <<= 16;
-#else
         value >>= 16;
+#else
+        value <<= 16;
 #endif
         return hash_64(value, FDB_HASH_BITS);
 }
@@ -536,7 +537,6 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
         }
 
         __skb_pull(skb, sizeof(struct vxlanhdr));
-        skb_postpull_rcsum(skb, eth_hdr(skb), sizeof(struct vxlanhdr));
 
         /* Is this VNI defined? */
         vni = ntohl(vxh->vx_vni) >> 8;
@@ -555,7 +555,6 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
         /* Re-examine inner Ethernet packet */
         oip = ip_hdr(skb);
         skb->protocol = eth_type_trans(skb, vxlan->dev);
-        skb_postpull_rcsum(skb, eth_hdr(skb), ETH_HLEN);
 
         /* Ignore packet loops (and multicast echo) */
         if (compare_ether_addr(eth_hdr(skb)->h_source,
@@ -567,6 +566,7 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 
         __skb_tunnel_rx(skb, vxlan->dev);
         skb_reset_network_header(skb);
+        skb->ip_summed = CHECKSUM_NONE;
 
         err = IP_ECN_decapsulate(oip, skb);
         if (unlikely(err)) {
@@ -622,46 +622,89 @@ static inline u8 vxlan_ecn_encap(u8 tos,
         return INET_ECN_encapsulate(tos, inner);
 }
 
+static __be32 vxlan_find_dst(struct vxlan_dev *vxlan, struct sk_buff *skb)
+{
+        const struct ethhdr *eth = (struct ethhdr *) skb->data;
+        const struct vxlan_fdb *f;
+
+        if (is_multicast_ether_addr(eth->h_dest))
+                return vxlan->gaddr;
+
+        f = vxlan_find_mac(vxlan, eth->h_dest);
+        if (f)
+                return f->remote_ip;
+        else
+                return vxlan->gaddr;
+
+}
+
+static void vxlan_sock_free(struct sk_buff *skb)
+{
+        sock_put(skb->sk);
+}
+
+/* On transmit, associate with the tunnel socket */
+static void vxlan_set_owner(struct net_device *dev, struct sk_buff *skb)
+{
+        struct vxlan_net *vn = net_generic(dev_net(dev), vxlan_net_id);
+        struct sock *sk = vn->sock->sk;
+
+        skb_orphan(skb);
+        sock_hold(sk);
+        skb->sk = sk;
+        skb->destructor = vxlan_sock_free;
+}
+
+/* Compute source port for outgoing packet
+ *   first choice to use L4 flow hash since it will spread
+ *     better and maybe available from hardware
+ *   secondary choice is to use jhash on the Ethernet header
+ */
+static u16 vxlan_src_port(const struct vxlan_dev *vxlan, struct sk_buff *skb)
+{
+        unsigned int range = (vxlan->port_max - vxlan->port_min) + 1;
+        u32 hash;
+
+        hash = skb_get_rxhash(skb);
+        if (!hash)
+                hash = jhash(skb->data, 2 * ETH_ALEN,
+                             (__force u32) skb->protocol);
+
+        return (((u64) hash * range) >> 32) + vxlan->port_min;
+}
+
 /* Transmit local packets over Vxlan
  *
  * Outer IP header inherits ECN and DF from inner header.
  * Outer UDP destination is the VXLAN assigned port.
- *           source port is based on hash of flow if available
- *                       otherwise use a random value
+ *           source port is based on hash of flow
  */
 static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 {
         struct vxlan_dev *vxlan = netdev_priv(dev);
         struct rtable *rt;
-        const struct ethhdr *eth;
         const struct iphdr *old_iph;
         struct iphdr *iph;
         struct vxlanhdr *vxh;
         struct udphdr *uh;
         struct flowi4 fl4;
-        struct vxlan_fdb *f;
         unsigned int pkt_len = skb->len;
-        u32 hash;
         __be32 dst;
+        __u16 src_port;
         __be16 df = 0;
         __u8 tos, ttl;
         int err;
 
+        dst = vxlan_find_dst(vxlan, skb);
+        if (!dst)
+                goto drop;
+
         /* Need space for new headers (invalidates iph ptr) */
         if (skb_cow_head(skb, VXLAN_HEADROOM))
                 goto drop;
 
-        eth = (void *)skb->data;
         old_iph = ip_hdr(skb);
 
-        if (!is_multicast_ether_addr(eth->h_dest) &&
-            (f = vxlan_find_mac(vxlan, eth->h_dest)))
-                dst = f->remote_ip;
-        else if (vxlan->gaddr) {
-                dst = vxlan->gaddr;
-        } else
-                goto drop;
-
         ttl = vxlan->ttl;
         if (!ttl && IN_MULTICAST(ntohl(dst)))
                 ttl = 1;
@@ -670,11 +713,15 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
         if (tos == 1)
                 tos = vxlan_get_dsfield(old_iph, skb);
 
-        hash = skb_get_rxhash(skb);
+        src_port = vxlan_src_port(vxlan, skb);
+
+        memset(&fl4, 0, sizeof(fl4));
+        fl4.flowi4_oif = vxlan->link;
+        fl4.flowi4_tos = RT_TOS(tos);
+        fl4.daddr = dst;
+        fl4.saddr = vxlan->saddr;
 
-        rt = ip_route_output_gre(dev_net(dev), &fl4, dst,
-                                 vxlan->saddr, vxlan->vni,
-                                 RT_TOS(tos), vxlan->link);
+        rt = ip_route_output_key(dev_net(dev), &fl4);
         if (IS_ERR(rt)) {
                 netdev_dbg(dev, "no route to %pI4\n", &dst);
                 dev->stats.tx_carrier_errors++;
@@ -703,7 +750,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
         uh = udp_hdr(skb);
 
         uh->dest = htons(vxlan_port);
-        uh->source = hash ? :random32();
+        uh->source = htons(src_port);
 
         uh->len = htons(skb->len);
         uh->check = 0;
@@ -716,10 +763,12 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
         iph->frag_off   = df;
         iph->protocol   = IPPROTO_UDP;
         iph->tos        = vxlan_ecn_encap(tos, old_iph, skb);
-        iph->daddr      = fl4.daddr;
+        iph->daddr      = dst;
         iph->saddr      = fl4.saddr;
         iph->ttl        = ttl ? : ip4_dst_hoplimit(&rt->dst);
 
+        vxlan_set_owner(dev, skb);
+
         /* See __IPTUNNEL_XMIT */
         skb->ip_summed = CHECKSUM_NONE;
         ip_select_ident(iph, &rt->dst, NULL);
@@ -767,7 +816,7 @@ static void vxlan_cleanup(unsigned long arg)
                                 = container_of(p, struct vxlan_fdb, hlist);
                         unsigned long timeout;
 
-                        if (f->state == NUD_PERMANENT)
+                        if (f->state & NUD_PERMANENT)
                                 continue;
 
                         timeout = f->used + vxlan->age_interval * HZ;
@@ -929,9 +978,11 @@ static void vxlan_setup(struct net_device *dev)
 {
         struct vxlan_dev *vxlan = netdev_priv(dev);
         unsigned h;
+        int low, high;
 
         eth_hw_addr_random(dev);
         ether_setup(dev);
+        dev->hard_header_len = ETH_HLEN + VXLAN_HEADROOM;
 
         dev->netdev_ops = &vxlan_netdev_ops;
         dev->destructor = vxlan_free;
@@ -948,6 +999,10 @@ static void vxlan_setup(struct net_device *dev)
         vxlan->age_timer.function = vxlan_cleanup;
         vxlan->age_timer.data = (unsigned long) vxlan;
 
+        inet_get_local_port_range(&low, &high);
+        vxlan->port_min = low;
+        vxlan->port_max = high;
+
         vxlan->dev = dev;
 
         for (h = 0; h < FDB_HASH_SIZE; ++h)
@@ -964,6 +1019,7 @@ static const struct nla_policy vxlan_policy[IFLA_VXLAN_MAX + 1] = {
         [IFLA_VXLAN_LEARNING]   = { .type = NLA_U8 },
         [IFLA_VXLAN_AGEING]     = { .type = NLA_U32 },
         [IFLA_VXLAN_LIMIT]      = { .type = NLA_U32 },
+        [IFLA_VXLAN_PORT_RANGE] = { .len  = sizeof(struct ifla_vxlan_port_range) },
 };
 
 static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
@@ -996,6 +1052,18 @@ static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
                         return -EADDRNOTAVAIL;
                 }
         }
+
+        if (data[IFLA_VXLAN_PORT_RANGE]) {
+                const struct ifla_vxlan_port_range *p
+                        = nla_data(data[IFLA_VXLAN_PORT_RANGE]);
+
+                if (ntohs(p->high) < ntohs(p->low)) {
+                        pr_debug("port range %u .. %u not valid\n",
+                                 ntohs(p->low), ntohs(p->high));
+                        return -EINVAL;
+                }
+        }
+
         return 0;
 }
 
@@ -1022,14 +1090,18 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
         if (data[IFLA_VXLAN_LOCAL])
                 vxlan->saddr = nla_get_be32(data[IFLA_VXLAN_LOCAL]);
 
-        if (data[IFLA_VXLAN_LINK]) {
-                vxlan->link = nla_get_u32(data[IFLA_VXLAN_LINK]);
+        if (data[IFLA_VXLAN_LINK] &&
+            (vxlan->link = nla_get_u32(data[IFLA_VXLAN_LINK]))) {
+                struct net_device *lowerdev
+                         = __dev_get_by_index(net, vxlan->link);
 
-                if (!tb[IFLA_MTU]) {
-                        struct net_device *lowerdev;
-                        lowerdev = __dev_get_by_index(net, vxlan->link);
-                        dev->mtu = lowerdev->mtu - VXLAN_HEADROOM;
+                if (!lowerdev) {
+                        pr_info("ifindex %d does not exist\n", vxlan->link);
+                        return -ENODEV;
                 }
+
+                if (!tb[IFLA_MTU])
+                        dev->mtu = lowerdev->mtu - VXLAN_HEADROOM;
         }
 
         if (data[IFLA_VXLAN_TOS])
@@ -1046,6 +1118,13 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
         if (data[IFLA_VXLAN_LIMIT])
                 vxlan->addrmax = nla_get_u32(data[IFLA_VXLAN_LIMIT]);
 
+        if (data[IFLA_VXLAN_PORT_RANGE]) {
+                const struct ifla_vxlan_port_range *p
+                        = nla_data(data[IFLA_VXLAN_PORT_RANGE]);
+                vxlan->port_min = ntohs(p->low);
+                vxlan->port_max = ntohs(p->high);
+        }
+
         err = register_netdevice(dev);
         if (!err)
                 hlist_add_head_rcu(&vxlan->hlist, vni_head(net, vxlan->vni));
@@ -1074,23 +1153,28 @@ static size_t vxlan_get_size(const struct net_device *dev)
                 nla_total_size(sizeof(__u8)) +  /* IFLA_VXLAN_LEARNING */
                 nla_total_size(sizeof(__u32)) + /* IFLA_VXLAN_AGEING */
                 nla_total_size(sizeof(__u32)) + /* IFLA_VXLAN_LIMIT */
+                nla_total_size(sizeof(struct ifla_vxlan_port_range)) +
                 0;
 }
 
 static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 {
         const struct vxlan_dev *vxlan = netdev_priv(dev);
+        struct ifla_vxlan_port_range ports = {
+                .low =  htons(vxlan->port_min),
+                .high = htons(vxlan->port_max),
+        };
 
         if (nla_put_u32(skb, IFLA_VXLAN_ID, vxlan->vni))
                 goto nla_put_failure;
 
-        if (vxlan->gaddr && nla_put_u32(skb, IFLA_VXLAN_GROUP, vxlan->gaddr))
+        if (vxlan->gaddr && nla_put_be32(skb, IFLA_VXLAN_GROUP, vxlan->gaddr))
                 goto nla_put_failure;
 
         if (vxlan->link && nla_put_u32(skb, IFLA_VXLAN_LINK, vxlan->link))
                 goto nla_put_failure;
 
-        if (vxlan->saddr && nla_put_u32(skb, IFLA_VXLAN_LOCAL, vxlan->saddr))
+        if (vxlan->saddr && nla_put_be32(skb, IFLA_VXLAN_LOCAL, vxlan->saddr))
                 goto nla_put_failure;
 
         if (nla_put_u8(skb, IFLA_VXLAN_TTL, vxlan->ttl) ||
@@ -1100,6 +1184,9 @@ static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
             nla_put_u32(skb, IFLA_VXLAN_LIMIT, vxlan->addrmax))
                 goto nla_put_failure;
 
+        if (nla_put(skb, IFLA_VXLAN_PORT_RANGE, sizeof(ports), &ports))
+                goto nla_put_failure;
+
         return 0;
 
 nla_put_failure:
diff --git a/drivers/net/wan/farsync.c b/drivers/net/wan/farsync.c
index 1a623183cbe5..b6271325f803 100644
--- a/drivers/net/wan/farsync.c
+++ b/drivers/net/wan/farsync.c
@@ -597,7 +597,7 @@ fst_q_work_item(u64 * queue, int card_index)
          * bottom half for the card.  Note the limitation of 64 cards.
          * That ought to be enough
          */
-        mask = 1 << card_index;
+        mask = (u64)1 << card_index;
         *queue |= mask;
         spin_unlock_irqrestore(&fst_work_q_lock, flags);
 }
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 9fd6d9a9942e..9f31cfa56cc0 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -1804,7 +1804,7 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
         int ret;
         struct ath5k_hw *ah = hw->priv;
-        struct ath5k_vif *avf = (void *)vif->drv_priv;
+        struct ath5k_vif *avf;
         struct sk_buff *skb;
 
         if (WARN_ON(!vif)) {
@@ -1819,6 +1819,7 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
                 goto out;
         }
 
+        avf = (void *)vif->drv_priv;
         ath5k_txbuf_free_skb(ah, avf->bbuf);
         avf->bbuf->skb = skb;
         ret = ath5k_beacon_setup(ah, avf->bbuf);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
index 89bf94d4d8a1..6f7cf49eff4d 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
@@ -534,107 +534,107 @@ static const u32 ar9300_2p2_baseband_core[][2] = {
 
 static const u32 ar9300Modes_high_power_tx_gain_table_2p2[][5] = {
         /* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
-        {0x0000a2dc, 0x000cfff0, 0x000cfff0, 0x03aaa352, 0x03aaa352},
-        {0x0000a2e0, 0x000f0000, 0x000f0000, 0x03ccc584, 0x03ccc584},
-        {0x0000a2e4, 0x03f00000, 0x03f00000, 0x03f0f800, 0x03f0f800},
+        {0x0000a2dc, 0x00033800, 0x00033800, 0x03aaa352, 0x03aaa352},
+        {0x0000a2e0, 0x0003c000, 0x0003c000, 0x03ccc584, 0x03ccc584},
+        {0x0000a2e4, 0x03fc0000, 0x03fc0000, 0x03f0f800, 0x03f0f800},
         {0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
         {0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
         {0x0000a500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
         {0x0000a504, 0x06000003, 0x06000003, 0x04000002, 0x04000002},
         {0x0000a508, 0x0a000020, 0x0a000020, 0x08000004, 0x08000004},
         {0x0000a50c, 0x10000023, 0x10000023, 0x0b000200, 0x0b000200},
-        {0x0000a510, 0x15000028, 0x15000028, 0x0f000202, 0x0f000202},
-        {0x0000a514, 0x1b00002b, 0x1b00002b, 0x12000400, 0x12000400},
-        {0x0000a518, 0x1f020028, 0x1f020028, 0x16000402, 0x16000402},
-        {0x0000a51c, 0x2502002b, 0x2502002b, 0x19000404, 0x19000404},
-        {0x0000a520, 0x2a04002a, 0x2a04002a, 0x1c000603, 0x1c000603},
-        {0x0000a524, 0x2e06002a, 0x2e06002a, 0x21000a02, 0x21000a02},
-        {0x0000a528, 0x3302202d, 0x3302202d, 0x25000a04, 0x25000a04},
-        {0x0000a52c, 0x3804202c, 0x3804202c, 0x28000a20, 0x28000a20},
-        {0x0000a530, 0x3c06202c, 0x3c06202c, 0x2c000e20, 0x2c000e20},
-        {0x0000a534, 0x4108202d, 0x4108202d, 0x30000e22, 0x30000e22},
-        {0x0000a538, 0x4506402d, 0x4506402d, 0x34000e24, 0x34000e24},
-        {0x0000a53c, 0x4906222d, 0x4906222d, 0x38001640, 0x38001640},
-        {0x0000a540, 0x4d062231, 0x4d062231, 0x3c001660, 0x3c001660},
-        {0x0000a544, 0x50082231, 0x50082231, 0x3f001861, 0x3f001861},
-        {0x0000a548, 0x5608422e, 0x5608422e, 0x43001a81, 0x43001a81},
-        {0x0000a54c, 0x5a08442e, 0x5a08442e, 0x47001a83, 0x47001a83},
-        {0x0000a550, 0x5e0a4431, 0x5e0a4431, 0x4a001c84, 0x4a001c84},
-        {0x0000a554, 0x640a4432, 0x640a4432, 0x4e001ce3, 0x4e001ce3},
-        {0x0000a558, 0x680a4434, 0x680a4434, 0x52001ce5, 0x52001ce5},
-        {0x0000a55c, 0x6c0a6434, 0x6c0a6434, 0x56001ce9, 0x56001ce9},
-        {0x0000a560, 0x6f0a6633, 0x6f0a6633, 0x5a001ceb, 0x5a001ceb},
-        {0x0000a564, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a568, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a56c, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a570, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a574, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a578, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a57c, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
+        {0x0000a510, 0x16000220, 0x16000220, 0x0f000202, 0x0f000202},
+        {0x0000a514, 0x1c000223, 0x1c000223, 0x12000400, 0x12000400},
+        {0x0000a518, 0x21002220, 0x21002220, 0x16000402, 0x16000402},
+        {0x0000a51c, 0x27002223, 0x27002223, 0x19000404, 0x19000404},
+        {0x0000a520, 0x2b022220, 0x2b022220, 0x1c000603, 0x1c000603},
+        {0x0000a524, 0x2f022222, 0x2f022222, 0x21000a02, 0x21000a02},
+        {0x0000a528, 0x34022225, 0x34022225, 0x25000a04, 0x25000a04},
+        {0x0000a52c, 0x3a02222a, 0x3a02222a, 0x28000a20, 0x28000a20},
+        {0x0000a530, 0x3e02222c, 0x3e02222c, 0x2c000e20, 0x2c000e20},
+        {0x0000a534, 0x4202242a, 0x4202242a, 0x30000e22, 0x30000e22},
+        {0x0000a538, 0x4702244a, 0x4702244a, 0x34000e24, 0x34000e24},
+        {0x0000a53c, 0x4b02244c, 0x4b02244c, 0x38001640, 0x38001640},
+        {0x0000a540, 0x4e02246c, 0x4e02246c, 0x3c001660, 0x3c001660},
+        {0x0000a544, 0x52022470, 0x52022470, 0x3f001861, 0x3f001861},
+        {0x0000a548, 0x55022490, 0x55022490, 0x43001a81, 0x43001a81},
+        {0x0000a54c, 0x59022492, 0x59022492, 0x47001a83, 0x47001a83},
+        {0x0000a550, 0x5d022692, 0x5d022692, 0x4a001c84, 0x4a001c84},
+        {0x0000a554, 0x61022892, 0x61022892, 0x4e001ce3, 0x4e001ce3},
+        {0x0000a558, 0x65024890, 0x65024890, 0x52001ce5, 0x52001ce5},
+        {0x0000a55c, 0x69024892, 0x69024892, 0x56001ce9, 0x56001ce9},
+        {0x0000a560, 0x6e024c92, 0x6e024c92, 0x5a001ceb, 0x5a001ceb},
+        {0x0000a564, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a568, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a56c, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a570, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a574, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a578, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a57c, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
         {0x0000a580, 0x00800000, 0x00800000, 0x00800000, 0x00800000},
         {0x0000a584, 0x06800003, 0x06800003, 0x04800002, 0x04800002},
         {0x0000a588, 0x0a800020, 0x0a800020, 0x08800004, 0x08800004},
         {0x0000a58c, 0x10800023, 0x10800023, 0x0b800200, 0x0b800200},
-        {0x0000a590, 0x15800028, 0x15800028, 0x0f800202, 0x0f800202},
-        {0x0000a594, 0x1b80002b, 0x1b80002b, 0x12800400, 0x12800400},
-        {0x0000a598, 0x1f820028, 0x1f820028, 0x16800402, 0x16800402},
-        {0x0000a59c, 0x2582002b, 0x2582002b, 0x19800404, 0x19800404},
-        {0x0000a5a0, 0x2a84002a, 0x2a84002a, 0x1c800603, 0x1c800603},
-        {0x0000a5a4, 0x2e86002a, 0x2e86002a, 0x21800a02, 0x21800a02},
-        {0x0000a5a8, 0x3382202d, 0x3382202d, 0x25800a04, 0x25800a04},
-        {0x0000a5ac, 0x3884202c, 0x3884202c, 0x28800a20, 0x28800a20},
-        {0x0000a5b0, 0x3c86202c, 0x3c86202c, 0x2c800e20, 0x2c800e20},
-        {0x0000a5b4, 0x4188202d, 0x4188202d, 0x30800e22, 0x30800e22},
-        {0x0000a5b8, 0x4586402d, 0x4586402d, 0x34800e24, 0x34800e24},
-        {0x0000a5bc, 0x4986222d, 0x4986222d, 0x38801640, 0x38801640},
-        {0x0000a5c0, 0x4d862231, 0x4d862231, 0x3c801660, 0x3c801660},
-        {0x0000a5c4, 0x50882231, 0x50882231, 0x3f801861, 0x3f801861},
-        {0x0000a5c8, 0x5688422e, 0x5688422e, 0x43801a81, 0x43801a81},
-        {0x0000a5cc, 0x5a88442e, 0x5a88442e, 0x47801a83, 0x47801a83},
-        {0x0000a5d0, 0x5e8a4431, 0x5e8a4431, 0x4a801c84, 0x4a801c84},
-        {0x0000a5d4, 0x648a4432, 0x648a4432, 0x4e801ce3, 0x4e801ce3},
-        {0x0000a5d8, 0x688a4434, 0x688a4434, 0x52801ce5, 0x52801ce5},
-        {0x0000a5dc, 0x6c8a6434, 0x6c8a6434, 0x56801ce9, 0x56801ce9},
-        {0x0000a5e0, 0x6f8a6633, 0x6f8a6633, 0x5a801ceb, 0x5a801ceb},
-        {0x0000a5e4, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5e8, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5ec, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5f0, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5f4, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5f8, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5fc, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
+        {0x0000a590, 0x16800220, 0x16800220, 0x0f800202, 0x0f800202},
+        {0x0000a594, 0x1c800223, 0x1c800223, 0x12800400, 0x12800400},
+        {0x0000a598, 0x21802220, 0x21802220, 0x16800402, 0x16800402},
+        {0x0000a59c, 0x27802223, 0x27802223, 0x19800404, 0x19800404},
+        {0x0000a5a0, 0x2b822220, 0x2b822220, 0x1c800603, 0x1c800603},
+        {0x0000a5a4, 0x2f822222, 0x2f822222, 0x21800a02, 0x21800a02},
+        {0x0000a5a8, 0x34822225, 0x34822225, 0x25800a04, 0x25800a04},
+        {0x0000a5ac, 0x3a82222a, 0x3a82222a, 0x28800a20, 0x28800a20},
+        {0x0000a5b0, 0x3e82222c, 0x3e82222c, 0x2c800e20, 0x2c800e20},
+        {0x0000a5b4, 0x4282242a, 0x4282242a, 0x30800e22, 0x30800e22},
+        {0x0000a5b8, 0x4782244a, 0x4782244a, 0x34800e24, 0x34800e24},
+        {0x0000a5bc, 0x4b82244c, 0x4b82244c, 0x38801640, 0x38801640},
+        {0x0000a5c0, 0x4e82246c, 0x4e82246c, 0x3c801660, 0x3c801660},
+        {0x0000a5c4, 0x52822470, 0x52822470, 0x3f801861, 0x3f801861},
+        {0x0000a5c8, 0x55822490, 0x55822490, 0x43801a81, 0x43801a81},
+        {0x0000a5cc, 0x59822492, 0x59822492, 0x47801a83, 0x47801a83},
+        {0x0000a5d0, 0x5d822692, 0x5d822692, 0x4a801c84, 0x4a801c84},
+        {0x0000a5d4, 0x61822892, 0x61822892, 0x4e801ce3, 0x4e801ce3},
+        {0x0000a5d8, 0x65824890, 0x65824890, 0x52801ce5, 0x52801ce5},
+        {0x0000a5dc, 0x69824892, 0x69824892, 0x56801ce9, 0x56801ce9},
+        {0x0000a5e0, 0x6e824c92, 0x6e824c92, 0x5a801ceb, 0x5a801ceb},
+        {0x0000a5e4, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5e8, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5ec, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5f0, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5f4, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5f8, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5fc, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
         {0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
         {0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-        {0x0000a608, 0x01804601, 0x01804601, 0x00000000, 0x00000000},
-        {0x0000a60c, 0x01804601, 0x01804601, 0x00000000, 0x00000000},
-        {0x0000a610, 0x01804601, 0x01804601, 0x00000000, 0x00000000},
-        {0x0000a614, 0x01804601, 0x01804601, 0x01404000, 0x01404000},
-        {0x0000a618, 0x01804601, 0x01804601, 0x01404501, 0x01404501},
-        {0x0000a61c, 0x01804601, 0x01804601, 0x02008501, 0x02008501},
-        {0x0000a620, 0x03408d02, 0x03408d02, 0x0280ca03, 0x0280ca03},
-        {0x0000a624, 0x0300cc03, 0x0300cc03, 0x03010c04, 0x03010c04},
-        {0x0000a628, 0x03410d04, 0x03410d04, 0x04014c04, 0x04014c04},
-        {0x0000a62c, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a630, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a634, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a638, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a63c, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000b2dc, 0x000cfff0, 0x000cfff0, 0x03aaa352, 0x03aaa352},
-        {0x0000b2e0, 0x000f0000, 0x000f0000, 0x03ccc584, 0x03ccc584},
-        {0x0000b2e4, 0x03f00000, 0x03f00000, 0x03f0f800, 0x03f0f800},
+        {0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+        {0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+        {0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+        {0x0000a614, 0x02004000, 0x02004000, 0x01404000, 0x01404000},
+        {0x0000a618, 0x02004801, 0x02004801, 0x01404501, 0x01404501},
+        {0x0000a61c, 0x02808a02, 0x02808a02, 0x02008501, 0x02008501},
+        {0x0000a620, 0x0380ce03, 0x0380ce03, 0x0280ca03, 0x0280ca03},
+        {0x0000a624, 0x04411104, 0x04411104, 0x03010c04, 0x03010c04},
+        {0x0000a628, 0x04411104, 0x04411104, 0x04014c04, 0x04014c04},
+        {0x0000a62c, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a630, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a634, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a638, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a63c, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000b2dc, 0x00033800, 0x00033800, 0x03aaa352, 0x03aaa352},
+        {0x0000b2e0, 0x0003c000, 0x0003c000, 0x03ccc584, 0x03ccc584},
+        {0x0000b2e4, 0x03fc0000, 0x03fc0000, 0x03f0f800, 0x03f0f800},
         {0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-        {0x0000c2dc, 0x000cfff0, 0x000cfff0, 0x03aaa352, 0x03aaa352},
-        {0x0000c2e0, 0x000f0000, 0x000f0000, 0x03ccc584, 0x03ccc584},
-        {0x0000c2e4, 0x03f00000, 0x03f00000, 0x03f0f800, 0x03f0f800},
+        {0x0000c2dc, 0x00033800, 0x00033800, 0x03aaa352, 0x03aaa352},
+        {0x0000c2e0, 0x0003c000, 0x0003c000, 0x03ccc584, 0x03ccc584},
+        {0x0000c2e4, 0x03fc0000, 0x03fc0000, 0x03f0f800, 0x03f0f800},
         {0x0000c2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
         {0x00016044, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-        {0x00016048, 0x61200001, 0x61200001, 0x66480001, 0x66480001},
+        {0x00016048, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
         {0x00016068, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
         {0x00016444, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-        {0x00016448, 0x61200001, 0x61200001, 0x66480001, 0x66480001},
+        {0x00016448, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
         {0x00016468, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
         {0x00016844, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-        {0x00016848, 0x61200001, 0x61200001, 0x66480001, 0x66480001},
+        {0x00016848, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
         {0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
 };
 
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 76f07d8c272d..1b48414dca95 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -120,7 +120,7 @@ static void ath9k_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
 
         if (ath_tx_start(hw, skb, &txctl) != 0) {
                 ath_dbg(common, XMIT, "CABQ TX failed\n");
-                dev_kfree_skb_any(skb);
+                ieee80211_free_txskb(hw, skb);
         }
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 924c4616c3d9..f5dda84176c3 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -38,6 +38,7 @@ static struct usb_device_id ath9k_hif_usb_ids[] = {
         { USB_DEVICE(0x04CA, 0x4605) }, /* Liteon */
         { USB_DEVICE(0x040D, 0x3801) }, /* VIA */
         { USB_DEVICE(0x0cf3, 0xb003) }, /* Ubiquiti WifiStation Ext */
+        { USB_DEVICE(0x0cf3, 0xb002) }, /* Ubiquiti WifiStation */
         { USB_DEVICE(0x057c, 0x8403) }, /* AVM FRITZ!WLAN 11N v2 USB */
 
         { USB_DEVICE(0x0cf3, 0x7015),
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index f9a6ec5cf470..8e1559aba495 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -1450,9 +1450,14 @@ static bool ath9k_hw_set_reset_reg(struct ath_hw *ah, u32 type)
         REG_WRITE(ah, AR_RTC_FORCE_WAKE,
                   AR_RTC_FORCE_WAKE_EN | AR_RTC_FORCE_WAKE_ON_INT);
 
+        if (!ah->reset_power_on)
+                type = ATH9K_RESET_POWER_ON;
+
         switch (type) {
         case ATH9K_RESET_POWER_ON:
                 ret = ath9k_hw_set_reset_power_on(ah);
+                if (!ret)
+                        ah->reset_power_on = true;
                 break;
         case ATH9K_RESET_WARM:
         case ATH9K_RESET_COLD:
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 566a4ce4f156..dbc1b7a4cbfd 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -741,6 +741,7 @@ struct ath_hw {
         u32 rfkill_polarity;
         u32 ah_flags;
 
+        bool reset_power_on;
         bool htc_reset_init;
 
         enum nl80211_iftype opmode;
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 31ab82e3ba85..dd45edfa6bae 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -639,8 +639,7 @@ static int ath9k_start(struct ieee80211_hw *hw)
                 ath_err(common,
                         "Unable to reset hardware; reset status %d (freq %u MHz)\n",
                         r, curchan->center_freq);
-                spin_unlock_bh(&sc->sc_pcu_lock);
-                goto mutex_unlock;
+                ah->reset_power_on = false;
         }
 
         /* Setup our intr mask. */
@@ -665,11 +664,8 @@ static int ath9k_start(struct ieee80211_hw *hw)
         clear_bit(SC_OP_INVALID, &sc->sc_flags);
         sc->sc_ah->is_monitoring = false;
 
-        if (!ath_complete_reset(sc, false)) {
-                r = -EIO;
-                spin_unlock_bh(&sc->sc_pcu_lock);
-                goto mutex_unlock;
-        }
+        if (!ath_complete_reset(sc, false))
+                ah->reset_power_on = false;
 
         if (ah->led_pin >= 0) {
                 ath9k_hw_cfg_output(ah, ah->led_pin,
@@ -688,12 +684,11 @@ static int ath9k_start(struct ieee80211_hw *hw)
         if (ah->caps.pcie_lcr_extsync_en && common->bus_ops->extn_synch_en)
                 common->bus_ops->extn_synch_en(common);
 
-mutex_unlock:
         mutex_unlock(&sc->mutex);
 
         ath9k_ps_restore(sc);
 
-        return r;
+        return 0;
 }
 
 static void ath9k_tx(struct ieee80211_hw *hw,
@@ -770,7 +765,7 @@ static void ath9k_tx(struct ieee80211_hw *hw,
 
         return;
 exit:
-        dev_kfree_skb_any(skb);
+        ieee80211_free_txskb(hw, skb);
 }
 
 static void ath9k_stop(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index 0e630a99b68b..f088f4bf9a26 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -324,6 +324,10 @@ static int ath_pci_suspend(struct device *device)
 static int ath_pci_resume(struct device *device)
 {
         struct pci_dev *pdev = to_pci_dev(device);
+        struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+        struct ath_softc *sc = hw->priv;
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath_common *common = ath9k_hw_common(ah);
         u32 val;
 
         /*
@@ -335,6 +339,9 @@ static int ath_pci_resume(struct device *device)
         if ((val & 0x0000ff00) != 0)
                 pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
 
+        ath_pci_aspm_init(common);
+        ah->reset_power_on = false;
+
         return 0;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 36618e3a5e60..741918a2027b 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -66,8 +66,7 @@ static void ath_tx_update_baw(struct ath_softc *sc, struct ath_atx_tid *tid,
 static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
                                            struct ath_txq *txq,
                                            struct ath_atx_tid *tid,
-                                           struct sk_buff *skb,
-                                           bool dequeue);
+                                           struct sk_buff *skb);
 
 enum {
         MCS_HT20,
@@ -176,7 +175,15 @@ static void ath_tx_flush_tid(struct ath_softc *sc, struct ath_atx_tid *tid)
                 fi = get_frame_info(skb);
                 bf = fi->bf;
 
-                if (bf && fi->retries) {
+                if (!bf) {
+                        bf = ath_tx_setup_buffer(sc, txq, tid, skb);
+                        if (!bf) {
+                                ieee80211_free_txskb(sc->hw, skb);
+                                continue;
+                        }
+                }
+
+                if (fi->retries) {
                         list_add_tail(&bf->list, &bf_head);
                         ath_tx_update_baw(sc, tid, bf->bf_state.seqno);
                         ath_tx_complete_buf(sc, bf, txq, &bf_head, &ts, 0);
@@ -305,6 +312,7 @@ static struct ath_buf *ath_tx_get_buffer(struct ath_softc *sc)
         }
 
         bf = list_first_entry(&sc->tx.txbuf, struct ath_buf, list);
+        bf->bf_next = NULL;
         list_del(&bf->list);
 
         spin_unlock_bh(&sc->tx.txbuflock);
@@ -386,7 +394,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
         u16 seq_st = 0, acked_cnt = 0, txfail_cnt = 0, seq_first;
         u32 ba[WME_BA_BMP_SIZE >> 5];
         int isaggr, txfail, txpending, sendbar = 0, needreset = 0, nbad = 0;
-        bool rc_update = true;
+        bool rc_update = true, isba;
         struct ieee80211_tx_rate rates[4];
         struct ath_frame_info *fi;
         int nframes;
@@ -430,13 +438,17 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
         tidno = ieee80211_get_qos_ctl(hdr)[0] & IEEE80211_QOS_CTL_TID_MASK;
         tid = ATH_AN_2_TID(an, tidno);
         seq_first = tid->seq_start;
+        isba = ts->ts_flags & ATH9K_TX_BA;
 
         /*
          * The hardware occasionally sends a tx status for the wrong TID.
          * In this case, the BA status cannot be considered valid and all
          * subframes need to be retransmitted
+         *
+         * Only BlockAcks have a TID and therefore normal Acks cannot be
+         * checked
          */
-        if (tidno != ts->tid)
+        if (isba && tidno != ts->tid)
                 txok = false;
 
         isaggr = bf_isaggr(bf);
@@ -785,10 +797,13 @@ static enum ATH_AGGR_STATUS ath_tx_form_aggr(struct ath_softc *sc,
                 fi = get_frame_info(skb);
                 bf = fi->bf;
                 if (!fi->bf)
-                        bf = ath_tx_setup_buffer(sc, txq, tid, skb, true);
+                        bf = ath_tx_setup_buffer(sc, txq, tid, skb);
 
-                if (!bf)
+                if (!bf) {
+                        __skb_unlink(skb, &tid->buf_q);
+                        ieee80211_free_txskb(sc->hw, skb);
                         continue;
+                }
 
                 bf->bf_state.bf_type = BUF_AMPDU | BUF_AGGR;
                 seqno = bf->bf_state.seqno;
@@ -1731,9 +1746,11 @@ static void ath_tx_send_ampdu(struct ath_softc *sc, struct ath_atx_tid *tid,
                 return;
         }
 
-        bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb, false);
-        if (!bf)
+        bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb);
+        if (!bf) {
+                ieee80211_free_txskb(sc->hw, skb);
                 return;
+        }
 
         bf->bf_state.bf_type = BUF_AMPDU;
         INIT_LIST_HEAD(&bf_head);
@@ -1757,16 +1774,12 @@ static void ath_tx_send_normal(struct ath_softc *sc, struct ath_txq *txq,
         struct ath_buf *bf;
 
         bf = fi->bf;
-        if (!bf)
-                bf = ath_tx_setup_buffer(sc, txq, tid, skb, false);
-
-        if (!bf)
-                return;
 
         INIT_LIST_HEAD(&bf_head);
         list_add_tail(&bf->list, &bf_head);
         bf->bf_state.bf_type = 0;
 
+        bf->bf_next = NULL;
         bf->bf_lastbf = bf;
         ath_tx_fill_desc(sc, bf, txq, fi->framelen);
         ath_tx_txqaddbuf(sc, txq, &bf_head, false);
@@ -1839,8 +1852,7 @@ u8 ath_txchainmask_reduction(struct ath_softc *sc, u8 chainmask, u32 rate)
 static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
                                            struct ath_txq *txq,
                                            struct ath_atx_tid *tid,
-                                           struct sk_buff *skb,
-                                           bool dequeue)
+                                           struct sk_buff *skb)
 {
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
         struct ath_frame_info *fi = get_frame_info(skb);
@@ -1852,7 +1864,7 @@ static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
         bf = ath_tx_get_buffer(sc);
         if (!bf) {
                 ath_dbg(common, XMIT, "TX buffers are full\n");
-                goto error;
+                return NULL;
         }
 
         ATH_TXBUF_RESET(bf);
@@ -1881,18 +1893,12 @@ static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
                 ath_err(ath9k_hw_common(sc->sc_ah),
                         "dma_mapping_error() on TX\n");
                 ath_tx_return_buffer(sc, bf);
-                goto error;
+                return NULL;
         }
 
         fi->bf = bf;
 
         return bf;
-
-error:
-        if (dequeue)
-                __skb_unlink(skb, &tid->buf_q);
-        dev_kfree_skb_any(skb);
-        return NULL;
 }
 
 /* FIXME: tx power */
@@ -1921,9 +1927,14 @@ static void ath_tx_start_dma(struct ath_softc *sc, struct sk_buff *skb,
                  */
                 ath_tx_send_ampdu(sc, tid, skb, txctl);
         } else {
-                bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb, false);
-                if (!bf)
+                bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb);
+                if (!bf) {
+                        if (txctl->paprd)
+                                dev_kfree_skb_any(skb);
+                        else
+                                ieee80211_free_txskb(sc->hw, skb);
                         return;
+                }
 
                 bf->bf_state.bfs_paprd = txctl->paprd;
 
diff --git a/drivers/net/wireless/ath/carl9170/carl9170.h b/drivers/net/wireless/ath/carl9170/carl9170.h
index 2aa4a59c72c8..2df17f1e49ef 100644
--- a/drivers/net/wireless/ath/carl9170/carl9170.h
+++ b/drivers/net/wireless/ath/carl9170/carl9170.h
@@ -303,6 +303,7 @@ struct ar9170 {
         unsigned long queue_stop_timeout[__AR9170_NUM_TXQ];
         unsigned long max_queue_stop_timeout[__AR9170_NUM_TXQ];
         bool needs_full_reset;
+        bool force_usb_reset;
         atomic_t pending_restarts;
 
         /* interface mode settings */
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index 67997b39aba7..25a1e2f4f738 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -465,27 +465,26 @@ static void carl9170_restart_work(struct work_struct *work)
 {
         struct ar9170 *ar = container_of(work, struct ar9170,
                                          restart_work);
-        int err;
+        int err = -EIO;
 
         ar->usedkeys = 0;
         ar->filter_state = 0;
         carl9170_cancel_worker(ar);
 
         mutex_lock(&ar->mutex);
-        err = carl9170_usb_restart(ar);
-        if (net_ratelimit()) {
-                if (err) {
-                        dev_err(&ar->udev->dev, "Failed to restart device "
-                                " (%d).\n", err);
-                 } else {
-                        dev_info(&ar->udev->dev, "device restarted "
-                                 "successfully.\n");
+        if (!ar->force_usb_reset) {
+                err = carl9170_usb_restart(ar);
+                if (net_ratelimit()) {
+                        if (err)
+                                dev_err(&ar->udev->dev, "Failed to restart device (%d).\n", err);
+                        else
+                                dev_info(&ar->udev->dev, "device restarted successfully.\n");
                 }
         }
-
         carl9170_zap_queues(ar);
         mutex_unlock(&ar->mutex);
-        if (!err) {
+
+        if (!err && !ar->force_usb_reset) {
                 ar->restart_counter++;
                 atomic_set(&ar->pending_restarts, 0);
 
@@ -526,10 +525,10 @@ void carl9170_restart(struct ar9170 *ar, const enum carl9170_restart_reasons r)
         if (!ar->registered)
                 return;
 
-        if (IS_ACCEPTING_CMD(ar) && !ar->needs_full_reset)
-                ieee80211_queue_work(ar->hw, &ar->restart_work);
-        else
-                carl9170_usb_reset(ar);
+        if (!IS_ACCEPTING_CMD(ar) || ar->needs_full_reset)
+                ar->force_usb_reset = true;
+
+        ieee80211_queue_work(ar->hw, &ar->restart_work);
 
         /*
          * At this point, the device instance might have vanished/disabled.
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 73730e94e0ac..c5a99c8c8168 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -5404,6 +5404,8 @@ static void b43_bcma_remove(struct bcma_device *core)
         cancel_work_sync(&wldev->restart_work);
 
         B43_WARN_ON(!wl);
+        if (!wldev->fw.ucode.data)
+                return;                 /* NULL if firmware never loaded */
         if (wl->current_dev == wldev && wl->hw_registred) {
                 b43_leds_stop(wldev);
                 ieee80211_unregister_hw(wl->hw);
@@ -5478,6 +5480,8 @@ static void b43_ssb_remove(struct ssb_device *sdev)
         cancel_work_sync(&wldev->restart_work);
 
         B43_WARN_ON(!wl);
+        if (!wldev->fw.ucode.data)
+                return;                 /* NULL if firmware never loaded */
         if (wl->current_dev == wldev && wl->hw_registred) {
                 b43_leds_stop(wldev);
                 ieee80211_unregister_hw(wl->hw);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
index a2b4b1e71017..7a6dfdc67b6c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
@@ -1339,7 +1339,7 @@ static int brcmf_usb_probe_cb(struct brcmf_usbdev_info *devinfo,
         }
 
         ret = brcmf_bus_start(dev);
-        if (ret == -ENOLINK) {
+        if (ret) {
                 brcmf_dbg(ERROR, "dongle is not responding\n");
                 brcmf_detach(dev);
                 goto fail;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index c1abaa6db59e..a6f1e8166008 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -3569,7 +3569,7 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
 
         if (!request || !request->n_ssids || !request->n_match_sets) {
                 WL_ERR("Invalid sched scan req!! n_ssids:%d\n",
-                       request->n_ssids);
+                       request ? request->n_ssids : 0);
                 return -EINVAL;
         }
 
@@ -3972,7 +3972,7 @@ brcmf_set_management_ie(struct brcmf_cfg80211_info *cfg,
         u8  *iovar_ie_buf;
         u8  *curr_ie_buf;
         u8  *mgmt_ie_buf = NULL;
-        u32 mgmt_ie_buf_len = 0;
+        int mgmt_ie_buf_len;
         u32 *mgmt_ie_len = 0;
         u32 del_add_ie_buf_len = 0;
         u32 total_ie_buf_len = 0;
@@ -3982,7 +3982,7 @@ brcmf_set_management_ie(struct brcmf_cfg80211_info *cfg,
         struct parsed_vndr_ie_info *vndrie_info;
         s32 i;
         u8 *ptr;
-        u32 remained_buf_len;
+        int remained_buf_len;
 
         WL_TRACE("bssidx %d, pktflag : 0x%02X\n", bssidx, pktflag);
         iovar_ie_buf = kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
@@ -4606,12 +4606,13 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
         struct brcmf_cfg80211_profile *profile = cfg->profile;
         struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg);
         struct wiphy *wiphy = cfg_to_wiphy(cfg);
-        struct brcmf_channel_info_le channel_le;
-        struct ieee80211_channel *notify_channel;
+        struct ieee80211_channel *notify_channel = NULL;
         struct ieee80211_supported_band *band;
+        struct brcmf_bss_info_le *bi;
         u32 freq;
         s32 err = 0;
         u32 target_channel;
+        u8 *buf;
 
         WL_TRACE("Enter\n");
 
@@ -4619,11 +4620,22 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
         memcpy(profile->bssid, e->addr, ETH_ALEN);
         brcmf_update_bss_info(cfg);
 
-        brcmf_exec_dcmd(ndev, BRCMF_C_GET_CHANNEL, &channel_le,
-                        sizeof(channel_le));
+        buf = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
+        if (buf == NULL) {
+                err = -ENOMEM;
+                goto done;
+        }
+
+        /* data sent to dongle has to be little endian */
+        *(__le32 *)buf = cpu_to_le32(WL_BSS_INFO_MAX);
+        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_BSS_INFO, buf, WL_BSS_INFO_MAX);
+
+        if (err)
+                goto done;
 
-        target_channel = le32_to_cpu(channel_le.target_channel);
-        WL_CONN("Roamed to channel %d\n", target_channel);
+        bi = (struct brcmf_bss_info_le *)(buf + 4);
+        target_channel = bi->ctl_ch ? bi->ctl_ch :
+                                      CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
 
         if (target_channel <= CH_MAX_2G_CHANNEL)
                 band = wiphy->bands[IEEE80211_BAND_2GHZ];
@@ -4633,6 +4645,8 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
         freq = ieee80211_channel_to_frequency(target_channel, band->band);
         notify_channel = ieee80211_get_channel(wiphy, freq);
 
+done:
+        kfree(buf);
         cfg80211_roamed(ndev, notify_channel, (u8 *)profile->bssid,
                         conn_info->req_ie, conn_info->req_ie_len,
                         conn_info->resp_ie, conn_info->resp_ie_len, GFP_KERNEL);
@@ -5186,41 +5200,6 @@ brcmf_cfg80211_event(struct net_device *ndev,
                 schedule_work(&cfg->event_work);
 }
 
-static s32 brcmf_dongle_mode(struct net_device *ndev, s32 iftype)
-{
-        s32 infra = 0;
-        s32 err = 0;
-
-        switch (iftype) {
-        case NL80211_IFTYPE_MONITOR:
-        case NL80211_IFTYPE_WDS:
-                WL_ERR("type (%d) : currently we do not support this mode\n",
-                       iftype);
-                err = -EINVAL;
-                return err;
-        case NL80211_IFTYPE_ADHOC:
-                infra = 0;
-                break;
-        case NL80211_IFTYPE_STATION:
-                infra = 1;
-                break;
-        case NL80211_IFTYPE_AP:
-                infra = 1;
-                break;
-        default:
-                err = -EINVAL;
-                WL_ERR("invalid type (%d)\n", iftype);
-                return err;
-        }
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_INFRA, &infra);
-        if (err) {
-                WL_ERR("WLC_SET_INFRA error (%d)\n", err);
-                return err;
-        }
-
-        return 0;
-}
-
 static s32 brcmf_dongle_eventmsg(struct net_device *ndev)
 {
         /* Room for "event_msgs" + '\0' + bitvec */
@@ -5439,7 +5418,8 @@ static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
                                 WL_BEACON_TIMEOUT);
         if (err)
                 goto default_conf_out;
-        err = brcmf_dongle_mode(ndev, wdev->iftype);
+        err = brcmf_cfg80211_change_iface(wdev->wiphy, ndev, wdev->iftype,
+                                          NULL, NULL);
         if (err && err != -EINPROGRESS)
                 goto default_conf_out;
         err = brcmf_dongle_probecap(cfg);
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index 935120fc8c93..768bf612533e 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -10472,7 +10472,7 @@ static void ipw_handle_promiscuous_tx(struct ipw_priv *priv,
                 } else
                         len = src->len;
 
-                dst = alloc_skb(len + sizeof(*rt_hdr), GFP_ATOMIC);
+                dst = alloc_skb(len + sizeof(*rt_hdr) + sizeof(u16)*2, GFP_ATOMIC);
                 if (!dst)
                         continue;
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/devices.c b/drivers/net/wireless/iwlwifi/dvm/devices.c
index 349c205d5f62..da5862064195 100644
--- a/drivers/net/wireless/iwlwifi/dvm/devices.c
+++ b/drivers/net/wireless/iwlwifi/dvm/devices.c
@@ -518,7 +518,7 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
          * See iwlagn_mac_channel_switch.
          */
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-        struct iwl6000_channel_switch_cmd cmd;
+        struct iwl6000_channel_switch_cmd *cmd;
         u32 switch_time_in_usec, ucode_switch_time;
         u16 ch;
         u32 tsf_low;
@@ -527,18 +527,25 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
         struct ieee80211_vif *vif = ctx->vif;
         struct iwl_host_cmd hcmd = {
                 .id = REPLY_CHANNEL_SWITCH,
-                .len = { sizeof(cmd), },
+                .len = { sizeof(*cmd), },
                 .flags = CMD_SYNC,
-                .data = { &cmd, },
+                .dataflags[0] = IWL_HCMD_DFL_NOCOPY,
         };
+        int err;
 
-        cmd.band = priv->band == IEEE80211_BAND_2GHZ;
+        cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+        if (!cmd)
+                return -ENOMEM;
+
+        hcmd.data[0] = cmd;
+
+        cmd->band = priv->band == IEEE80211_BAND_2GHZ;
         ch = ch_switch->channel->hw_value;
         IWL_DEBUG_11H(priv, "channel switch from %u to %u\n",
                       ctx->active.channel, ch);
-        cmd.channel = cpu_to_le16(ch);
-        cmd.rxon_flags = ctx->staging.flags;
-        cmd.rxon_filter_flags = ctx->staging.filter_flags;
+        cmd->channel = cpu_to_le16(ch);
+        cmd->rxon_flags = ctx->staging.flags;
+        cmd->rxon_filter_flags = ctx->staging.filter_flags;
         switch_count = ch_switch->count;
         tsf_low = ch_switch->timestamp & 0x0ffffffff;
         /*
@@ -554,23 +561,25 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
                         switch_count = 0;
         }
         if (switch_count <= 1)
-                cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+                cmd->switch_time = cpu_to_le32(priv->ucode_beacon_time);
         else {
                 switch_time_in_usec =
                         vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
                 ucode_switch_time = iwl_usecs_to_beacons(priv,
                                                          switch_time_in_usec,
                                                          beacon_interval);
-                cmd.switch_time = iwl_add_beacon_time(priv,
-                                                      priv->ucode_beacon_time,
-                                                      ucode_switch_time,
-                                                      beacon_interval);
+                cmd->switch_time = iwl_add_beacon_time(priv,
+                                                       priv->ucode_beacon_time,
+                                                       ucode_switch_time,
+                                                       beacon_interval);
         }
         IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
-                      cmd.switch_time);
-        cmd.expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
+                      cmd->switch_time);
+        cmd->expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
 
-        return iwl_dvm_send_cmd(priv, &hcmd);
+        err = iwl_dvm_send_cmd(priv, &hcmd);
+        kfree(cmd);
+        return err;
 }
 
 struct iwl_lib_ops iwl6000_lib = {
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 2691620393ea..780d3e168297 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -1596,8 +1596,9 @@ done:
                 }
         }
 
-        if (mwifiex_bss_start(priv, bss, &req_ssid))
-                return -EFAULT;
+        ret = mwifiex_bss_start(priv, bss, &req_ssid);
+        if (ret)
+                return ret;
 
         if (mode == NL80211_IFTYPE_ADHOC) {
                 /* Inform the BSS information to kernel, otherwise
@@ -1652,9 +1653,19 @@ done:
                         "info: association to bssid %pM failed\n",
                         priv->cfg_bssid);
                 memset(priv->cfg_bssid, 0, ETH_ALEN);
+
+                if (ret > 0)
+                        cfg80211_connect_result(priv->netdev, priv->cfg_bssid,
+                                                NULL, 0, NULL, 0, ret,
+                                                GFP_KERNEL);
+                else
+                        cfg80211_connect_result(priv->netdev, priv->cfg_bssid,
+                                                NULL, 0, NULL, 0,
+                                                WLAN_STATUS_UNSPECIFIED_FAILURE,
+                                                GFP_KERNEL);
         }
 
-        return ret;
+        return 0;
 }
 
 /*
@@ -1802,7 +1813,7 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 {
         struct net_device *dev = request->wdev->netdev;
         struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-        int i, offset;
+        int i, offset, ret;
         struct ieee80211_channel *chan;
         struct ieee_types_header *ie;
 
@@ -1814,8 +1825,6 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
                 return -EBUSY;
         }
 
-        priv->scan_request = request;
-
         priv->user_scan_cfg = kzalloc(sizeof(struct mwifiex_user_scan_cfg),
                                       GFP_KERNEL);
         if (!priv->user_scan_cfg) {
@@ -1823,6 +1832,8 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
                 return -ENOMEM;
         }
 
+        priv->scan_request = request;
+
         priv->user_scan_cfg->num_ssids = request->n_ssids;
         priv->user_scan_cfg->ssid_list = request->ssids;
 
@@ -1855,8 +1866,15 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 
                 priv->user_scan_cfg->chan_list[i].scan_time = 0;
         }
-        if (mwifiex_scan_networks(priv, priv->user_scan_cfg))
-                return -EFAULT;
+
+        ret = mwifiex_scan_networks(priv, priv->user_scan_cfg);
+        if (ret) {
+                dev_err(priv->adapter->dev, "scan failed: %d\n", ret);
+                priv->scan_request = NULL;
+                kfree(priv->user_scan_cfg);
+                priv->user_scan_cfg = NULL;
+                return ret;
+        }
 
         if (request->ie && request->ie_len) {
                 for (i = 0; i < MWIFIEX_MAX_VSIE_NUM; i++) {
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 82e63cee1e97..7b0858af8f5d 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -1180,16 +1180,18 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
         struct mwifiex_adapter *adapter = priv->adapter;
         struct host_cmd_ds_802_11_ad_hoc_result *adhoc_result;
         struct mwifiex_bssdescriptor *bss_desc;
+        u16 reason_code;
 
         adhoc_result = &resp->params.adhoc_result;
 
         bss_desc = priv->attempted_bss_desc;
 
         /* Join result code 0 --> SUCCESS */
-        if (le16_to_cpu(resp->result)) {
+        reason_code = le16_to_cpu(resp->result);
+        if (reason_code) {
                 dev_err(priv->adapter->dev, "ADHOC_RESP: failed\n");
                 if (priv->media_connected)
-                        mwifiex_reset_connect_state(priv);
+                        mwifiex_reset_connect_state(priv, reason_code);
 
                 memset(&priv->curr_bss_params.bss_descriptor,
                        0x00, sizeof(struct mwifiex_bssdescriptor));
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index bfb3fa69805c..c2d0ab146af5 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -847,7 +847,7 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
                                  struct mwifiex_bssdescriptor *bss_desc);
 int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
                                  struct host_cmd_ds_command *resp);
-void mwifiex_reset_connect_state(struct mwifiex_private *priv);
+void mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason);
 u8 mwifiex_band_to_radio_type(u8 band);
 int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac);
 int mwifiex_adhoc_start(struct mwifiex_private *priv,
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index e36a75988f87..9171aaedbccd 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -1296,7 +1296,7 @@ mwifiex_radio_type_to_band(u8 radio_type)
 int mwifiex_scan_networks(struct mwifiex_private *priv,
                           const struct mwifiex_user_scan_cfg *user_scan_in)
 {
-        int ret = 0;
+        int ret;
         struct mwifiex_adapter *adapter = priv->adapter;
         struct cmd_ctrl_node *cmd_node;
         union mwifiex_scan_cmd_config_tlv *scan_cfg_out;
@@ -1309,25 +1309,26 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
         unsigned long flags;
 
         if (adapter->scan_processing) {
-                dev_dbg(adapter->dev, "cmd: Scan already in process...\n");
-                return ret;
+                dev_err(adapter->dev, "cmd: Scan already in process...\n");
+                return -EBUSY;
         }
 
-        spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-        adapter->scan_processing = true;
-        spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
-
         if (priv->scan_block) {
-                dev_dbg(adapter->dev,
+                dev_err(adapter->dev,
                         "cmd: Scan is blocked during association...\n");
-                return ret;
+                return -EBUSY;
         }
 
+        spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+        adapter->scan_processing = true;
+        spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+
         scan_cfg_out = kzalloc(sizeof(union mwifiex_scan_cmd_config_tlv),
                                                                 GFP_KERNEL);
         if (!scan_cfg_out) {
                 dev_err(adapter->dev, "failed to alloc scan_cfg_out\n");
-                return -ENOMEM;
+                ret = -ENOMEM;
+                goto done;
         }
 
         buf_size = sizeof(struct mwifiex_chan_scan_param_set) *
@@ -1336,7 +1337,8 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
         if (!scan_chan_list) {
                 dev_err(adapter->dev, "failed to alloc scan_chan_list\n");
                 kfree(scan_cfg_out);
-                return -ENOMEM;
+                ret = -ENOMEM;
+                goto done;
         }
 
         mwifiex_config_scan(priv, user_scan_in, &scan_cfg_out->config,
@@ -1364,14 +1366,16 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
                         spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
                                                flags);
                 }
-        } else {
-                spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-                adapter->scan_processing = true;
-                spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
         }
 
         kfree(scan_cfg_out);
         kfree(scan_chan_list);
+done:
+        if (ret) {
+                spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+                adapter->scan_processing = false;
+                spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+        }
         return ret;
 }
 
@@ -1430,8 +1434,8 @@ int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
                         ret = mwifiex_is_network_compatible(priv, bss_desc,
                                                             priv->bss_mode);
                         if (ret)
-                                dev_err(priv->adapter->dev, "cannot find ssid "
-                                        "%s\n", bss_desc->ssid.ssid);
+                                dev_err(priv->adapter->dev,
+                                        "Incompatible network settings\n");
                         break;
                 default:
                         ret = 0;
@@ -1839,21 +1843,18 @@ static int mwifiex_scan_specific_ssid(struct mwifiex_private *priv,
                                       struct cfg80211_ssid *req_ssid)
 {
         struct mwifiex_adapter *adapter = priv->adapter;
-        int ret = 0;
+        int ret;
         struct mwifiex_user_scan_cfg *scan_cfg;
 
-        if (!req_ssid)
-                return -1;
-
         if (adapter->scan_processing) {
-                dev_dbg(adapter->dev, "cmd: Scan already in process...\n");
-                return ret;
+                dev_err(adapter->dev, "cmd: Scan already in process...\n");
+                return -EBUSY;
         }
 
         if (priv->scan_block) {
-                dev_dbg(adapter->dev,
+                dev_err(adapter->dev,
                         "cmd: Scan is blocked during association...\n");
-                return ret;
+                return -EBUSY;
         }
 
         scan_cfg = kzalloc(sizeof(struct mwifiex_user_scan_cfg), GFP_KERNEL);
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index e380171c4c5d..09e6a267f566 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -545,7 +545,7 @@ static int mwifiex_ret_802_11_deauthenticate(struct mwifiex_private *priv,
         if (!memcmp(resp->params.deauth.mac_addr,
                     &priv->curr_bss_params.bss_descriptor.mac_address,
                     sizeof(resp->params.deauth.mac_addr)))
-                mwifiex_reset_connect_state(priv);
+                mwifiex_reset_connect_state(priv, WLAN_REASON_DEAUTH_LEAVING);
 
         return 0;
 }
@@ -558,7 +558,7 @@ static int mwifiex_ret_802_11_deauthenticate(struct mwifiex_private *priv,
 static int mwifiex_ret_802_11_ad_hoc_stop(struct mwifiex_private *priv,
                                           struct host_cmd_ds_command *resp)
 {
-        mwifiex_reset_connect_state(priv);
+        mwifiex_reset_connect_state(priv, WLAN_REASON_DEAUTH_LEAVING);
         return 0;
 }
 
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index aafde30e714a..8132119e1a21 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -41,7 +41,7 @@
  *      - Sends a disconnect event to upper layers/applications.
  */
 void
-mwifiex_reset_connect_state(struct mwifiex_private *priv)
+mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 {
         struct mwifiex_adapter *adapter = priv->adapter;
 
@@ -117,10 +117,10 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv)
         priv->media_connected = false;
         dev_dbg(adapter->dev,
                 "info: successfully disconnected from %pM: reason code %d\n",
-                priv->cfg_bssid, WLAN_REASON_DEAUTH_LEAVING);
+                priv->cfg_bssid, reason_code);
         if (priv->bss_mode == NL80211_IFTYPE_STATION) {
-                cfg80211_disconnected(priv->netdev, WLAN_REASON_DEAUTH_LEAVING,
-                                      NULL, 0, GFP_KERNEL);
+                cfg80211_disconnected(priv->netdev, reason_code, NULL, 0,
+                                      GFP_KERNEL);
         }
         memset(priv->cfg_bssid, 0, ETH_ALEN);
 
@@ -186,7 +186,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
         struct mwifiex_adapter *adapter = priv->adapter;
         int ret = 0;
         u32 eventcause = adapter->event_cause;
-        u16 ctrl;
+        u16 ctrl, reason_code;
 
         switch (eventcause) {
         case EVENT_DUMMY_HOST_WAKEUP_SIGNAL:
@@ -204,22 +204,31 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
         case EVENT_DEAUTHENTICATED:
                 dev_dbg(adapter->dev, "event: Deauthenticated\n");
                 adapter->dbg.num_event_deauth++;
-                if (priv->media_connected)
-                        mwifiex_reset_connect_state(priv);
+                if (priv->media_connected) {
+                        reason_code =
+                                le16_to_cpu(*(__le16 *)adapter->event_body);
+                        mwifiex_reset_connect_state(priv, reason_code);
+                }
                 break;
 
         case EVENT_DISASSOCIATED:
                 dev_dbg(adapter->dev, "event: Disassociated\n");
                 adapter->dbg.num_event_disassoc++;
-                if (priv->media_connected)
-                        mwifiex_reset_connect_state(priv);
+                if (priv->media_connected) {
+                        reason_code =
+                                le16_to_cpu(*(__le16 *)adapter->event_body);
+                        mwifiex_reset_connect_state(priv, reason_code);
+                }
                 break;
 
         case EVENT_LINK_LOST:
                 dev_dbg(adapter->dev, "event: Link lost\n");
                 adapter->dbg.num_event_link_lost++;
-                if (priv->media_connected)
-                        mwifiex_reset_connect_state(priv);
+                if (priv->media_connected) {
+                        reason_code =
+                                le16_to_cpu(*(__le16 *)adapter->event_body);
+                        mwifiex_reset_connect_state(priv, reason_code);
+                }
                 break;
 
         case EVENT_PS_SLEEP:
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index a12e84f892be..6b2e1e431dd2 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1988,6 +1988,7 @@ static struct usb_driver rt2500usb_driver = {
         .disconnect     = rt2x00usb_disconnect,
         .suspend        = rt2x00usb_suspend,
         .resume         = rt2x00usb_resume,
+        .reset_resume   = rt2x00usb_resume,
         .disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 540c94f8505a..59474ae0aec0 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -2252,9 +2252,9 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
          */
         if (rt2x00_rt(rt2x00dev, RT3352)) {
                 rt2800_bbp_write(rt2x00dev, 27, 0x0);
-                rt2800_bbp_write(rt2x00dev, 62, 0x26 + rt2x00dev->lna_gain);
+                rt2800_bbp_write(rt2x00dev, 66, 0x26 + rt2x00dev->lna_gain);
                 rt2800_bbp_write(rt2x00dev, 27, 0x20);
-                rt2800_bbp_write(rt2x00dev, 62, 0x26 + rt2x00dev->lna_gain);
+                rt2800_bbp_write(rt2x00dev, 66, 0x26 + rt2x00dev->lna_gain);
         } else {
                 rt2800_bbp_write(rt2x00dev, 62, 0x37 - rt2x00dev->lna_gain);
                 rt2800_bbp_write(rt2x00dev, 63, 0x37 - rt2x00dev->lna_gain);
@@ -2449,7 +2449,7 @@ static int rt2800_get_gain_calibration_delta(struct rt2x00_dev *rt2x00dev)
         /*
          * Check if temperature compensation is supported.
          */
-        if (tssi_bounds[4] == 0xff)
+        if (tssi_bounds[4] == 0xff || step == 0xff)
                 return 0;
 
         /*
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index c9e9370eb789..3b8fb5a603f2 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -1282,6 +1282,7 @@ static struct usb_driver rt2800usb_driver = {
         .disconnect     = rt2x00usb_disconnect,
         .suspend        = rt2x00usb_suspend,
         .resume         = rt2x00usb_resume,
+        .reset_resume   = rt2x00usb_resume,
         .disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index e5eb43b3eee7..24eec66e9fd2 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -2535,6 +2535,7 @@ static struct usb_driver rt73usb_driver = {
         .disconnect     = rt2x00usb_disconnect,
         .suspend        = rt2x00usb_suspend,
         .resume         = rt2x00usb_resume,
+        .reset_resume   = rt2x00usb_resume,
         .disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 030beb45d8b0..e3ea4b346889 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -673,7 +673,7 @@ static int rtl_usb_start(struct ieee80211_hw *hw)
                 set_hal_start(rtlhal);
 
                 /* Start bulk IN */
-                _rtl_usb_receive(hw);
+                err = _rtl_usb_receive(hw);
         }
 
         return err;
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 4ebfcf3d8a3b..f2d6b78d901d 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -335,21 +335,35 @@ unsigned int xen_netbk_count_skb_slots(struct xenvif *vif, struct sk_buff *skb)
 
         for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
                 unsigned long size = skb_frag_size(&skb_shinfo(skb)->frags[i]);
+                unsigned long offset = skb_shinfo(skb)->frags[i].page_offset;
                 unsigned long bytes;
+
+                offset &= ~PAGE_MASK;
+
                 while (size > 0) {
+                        BUG_ON(offset >= PAGE_SIZE);
                         BUG_ON(copy_off > MAX_BUFFER_OFFSET);
 
-                        if (start_new_rx_buffer(copy_off, size, 0)) {
+                        bytes = PAGE_SIZE - offset;
+
+                        if (bytes > size)
+                                bytes = size;
+
+                        if (start_new_rx_buffer(copy_off, bytes, 0)) {
                                 count++;
                                 copy_off = 0;
                         }
 
-                        bytes = size;
                         if (copy_off + bytes > MAX_BUFFER_OFFSET)
                                 bytes = MAX_BUFFER_OFFSET - copy_off;
 
                         copy_off += bytes;
+
+                        offset += bytes;
                         size -= bytes;
+
+                        if (offset == PAGE_SIZE)
+                                offset = 0;
                 }
         }
         return count;
@@ -403,14 +417,24 @@ static void netbk_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
         unsigned long bytes;
 
         /* Data must not cross a page boundary. */
-        BUG_ON(size + offset > PAGE_SIZE);
+        BUG_ON(size + offset > PAGE_SIZE<<compound_order(page));
 
         meta = npo->meta + npo->meta_prod - 1;
 
+        /* Skip unused frames from start of page */
+        page += offset >> PAGE_SHIFT;
+        offset &= ~PAGE_MASK;
+
         while (size > 0) {
+                BUG_ON(offset >= PAGE_SIZE);
                 BUG_ON(npo->copy_off > MAX_BUFFER_OFFSET);
 
-                if (start_new_rx_buffer(npo->copy_off, size, *head)) {
+                bytes = PAGE_SIZE - offset;
+
+                if (bytes > size)
+                        bytes = size;
+
+                if (start_new_rx_buffer(npo->copy_off, bytes, *head)) {
                         /*
                          * Netfront requires there to be some data in the head
                          * buffer.
@@ -420,7 +444,6 @@ static void netbk_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
                         meta = get_next_rx_buffer(vif, npo);
                 }
 
-                bytes = size;
                 if (npo->copy_off + bytes > MAX_BUFFER_OFFSET)
                         bytes = MAX_BUFFER_OFFSET - npo->copy_off;
 
@@ -453,6 +476,13 @@ static void netbk_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
                 offset += bytes;
                 size -= bytes;
 
+                /* Next frame */
+                if (offset == PAGE_SIZE && size) {
+                        BUG_ON(!PageCompound(page));
+                        page++;
+                        offset = 0;
+                }
+
                 /* Leave a gap for the GSO descriptor. */
                 if (*head && skb_shinfo(skb)->gso_size && !vif->gso_prefix)
                         vif->rx.req_cons++;
diff --git a/drivers/of/address.c b/drivers/of/address.c
index 72e496f1e9b0..0125524c08c4 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -37,9 +37,9 @@ struct of_bus {
         int             (*match)(struct device_node *parent);
         void            (*count_cells)(struct device_node *child,
                                        int *addrc, int *sizec);
-        u64             (*map)(u32 *addr, const __be32 *range,
+        u64             (*map)(__be32 *addr, const __be32 *range,
                                 int na, int ns, int pna);
-        int             (*translate)(u32 *addr, u64 offset, int na);
+        int             (*translate)(__be32 *addr, u64 offset, int na);
         unsigned int    (*get_flags)(const __be32 *addr);
 };
 
@@ -56,7 +56,7 @@ static void of_bus_default_count_cells(struct device_node *dev,
                 *sizec = of_n_size_cells(dev);
 }
 
-static u64 of_bus_default_map(u32 *addr, const __be32 *range,
+static u64 of_bus_default_map(__be32 *addr, const __be32 *range,
                 int na, int ns, int pna)
 {
         u64 cp, s, da;
@@ -82,7 +82,7 @@ static u64 of_bus_default_map(u32 *addr, const __be32 *range,
         return da - cp;
 }
 
-static int of_bus_default_translate(u32 *addr, u64 offset, int na)
+static int of_bus_default_translate(__be32 *addr, u64 offset, int na)
 {
         u64 a = of_read_number(addr, na);
         memset(addr, 0, na * 4);
@@ -138,7 +138,7 @@ static unsigned int of_bus_pci_get_flags(const __be32 *addr)
         return flags;
 }
 
-static u64 of_bus_pci_map(u32 *addr, const __be32 *range, int na, int ns,
+static u64 of_bus_pci_map(__be32 *addr, const __be32 *range, int na, int ns,
                 int pna)
 {
         u64 cp, s, da;
@@ -165,7 +165,7 @@ static u64 of_bus_pci_map(u32 *addr, const __be32 *range, int na, int ns,
         return da - cp;
 }
 
-static int of_bus_pci_translate(u32 *addr, u64 offset, int na)
+static int of_bus_pci_translate(__be32 *addr, u64 offset, int na)
 {
         return of_bus_default_translate(addr + 1, offset, na - 1);
 }
@@ -247,7 +247,7 @@ static void of_bus_isa_count_cells(struct device_node *child,
                 *sizec = 1;
 }
 
-static u64 of_bus_isa_map(u32 *addr, const __be32 *range, int na, int ns,
+static u64 of_bus_isa_map(__be32 *addr, const __be32 *range, int na, int ns,
                 int pna)
 {
         u64 cp, s, da;
@@ -270,7 +270,7 @@ static u64 of_bus_isa_map(u32 *addr, const __be32 *range, int na, int ns,
         return da - cp;
 }
 
-static int of_bus_isa_translate(u32 *addr, u64 offset, int na)
+static int of_bus_isa_translate(__be32 *addr, u64 offset, int na)
 {
         return of_bus_default_translate(addr + 1, offset, na - 1);
 }
@@ -338,7 +338,7 @@ static struct of_bus *of_match_bus(struct device_node *np)
 }
 
 static int of_translate_one(struct device_node *parent, struct of_bus *bus,
-                            struct of_bus *pbus, u32 *addr,
+                            struct of_bus *pbus, __be32 *addr,
                             int na, int ns, int pna, const char *rprop)
 {
         const __be32 *ranges;
@@ -409,12 +409,12 @@ static int of_translate_one(struct device_node *parent, struct of_bus *bus,
  * that can be mapped to a cpu physical address). This is not really specified
  * that way, but this is traditionally the way IBM at least do things
  */
-u64 __of_translate_address(struct device_node *dev, const __be32 *in_addr,
-                           const char *rprop)
+static u64 __of_translate_address(struct device_node *dev,
+                                  const __be32 *in_addr, const char *rprop)
 {
         struct device_node *parent = NULL;
         struct of_bus *bus, *pbus;
-        u32 addr[OF_MAX_ADDR_CELLS];
+        __be32 addr[OF_MAX_ADDR_CELLS];
         int na, ns, pna, pns;
         u64 result = OF_BAD_ADDR;
 
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index a23ec7779997..a3c1c5aae6a9 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -192,11 +192,13 @@ int of_irq_map_raw(struct device_node *parent, const __be32 *intspec,
                         /* Compare specifiers */
                         match = 1;
                         for (i = 0; i < addrsize && match; ++i) {
-                                u32 mask = imask ? imask[i] : 0xffffffffu;
+                                __be32 mask = imask ? imask[i]
+                                                    : cpu_to_be32(0xffffffffu);
                                 match = ((addr[i] ^ imap[i]) & mask) == 0;
                         }
                         for (; i < (addrsize + intsize) && match; ++i) {
-                                u32 mask = imask ? imask[i] : 0xffffffffu;
+                                __be32 mask = imask ? imask[i]
+                                                    : cpu_to_be32(0xffffffffu);
                                 match =
                                    ((intspec[i-addrsize] ^ imap[i]) & mask) == 0;
                         }
@@ -465,7 +467,7 @@ void __init of_irq_init(const struct of_device_id *matches)
                         pr_debug("of_irq_init: init %s @ %p, parent %p\n",
                                  match->compatible,
                                  desc->dev, desc->interrupt_parent);
-                        irq_init_cb = match->data;
+                        irq_init_cb = (of_irq_init_cb_t)match->data;
                         ret = irq_init_cb(desc->dev, desc->interrupt_parent);
                         if (ret) {
                                 kfree(desc);
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index 9bdeaf30b17d..b80891b43816 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -76,7 +76,7 @@ void of_device_make_bus_id(struct device *dev)
 {
         static atomic_t bus_no_reg_magic;
         struct device_node *node = dev->of_node;
-        const u32 *reg;
+        const __be32 *reg;
         u64 addr;
         const __be32 *addrp;
         int magic;
diff --git a/drivers/pcmcia/pxa2xx_sharpsl.c b/drivers/pcmcia/pxa2xx_sharpsl.c
index b066273b6b4f..7dd879ce514d 100644
--- a/drivers/pcmcia/pxa2xx_sharpsl.c
+++ b/drivers/pcmcia/pxa2xx_sharpsl.c
@@ -194,7 +194,7 @@ static void sharpsl_pcmcia_socket_suspend(struct soc_pcmcia_socket *skt)
         sharpsl_pcmcia_init_reset(skt);
 }
 
-static struct pcmcia_low_level sharpsl_pcmcia_ops __initdata = {
+static struct pcmcia_low_level sharpsl_pcmcia_ops = {
         .owner                  = THIS_MODULE,
         .hw_init                = sharpsl_pcmcia_hw_init,
         .socket_state           = sharpsl_pcmcia_socket_state,
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index 0f1ec9e8ff14..2e39c04fc16b 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -1061,8 +1061,10 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
                         seq_printf(s, "group: %s\n", gname);
                         for (i = 0; i < num_pins; i++) {
                                 pname = pin_get_name(pctldev, pins[i]);
-                                if (WARN_ON(!pname))
+                                if (WARN_ON(!pname)) {
+                                        mutex_unlock(&pinctrl_mutex);
                                         return -EINVAL;
+                                }
                                 seq_printf(s, "pin %d (%s)\n", pins[i], pname);
                         }
                         seq_puts(s, "\n");
diff --git a/drivers/pinctrl/pinconf.c b/drivers/pinctrl/pinconf.c
index 43f474cdc110..baee2cc46a17 100644
--- a/drivers/pinctrl/pinconf.c
+++ b/drivers/pinctrl/pinconf.c
@@ -537,8 +537,6 @@ static int pinconf_groups_show(struct seq_file *s, void *what)
         seq_puts(s, "Pin config settings per pin group\n");
         seq_puts(s, "Format: group (name): configs\n");
 
-        mutex_lock(&pinctrl_mutex);
-
         while (selector < ngroups) {
                 const char *gname = pctlops->get_group_name(pctldev, selector);
 
@@ -549,8 +547,6 @@ static int pinconf_groups_show(struct seq_file *s, void *what)
                 selector++;
         }
 
-        mutex_unlock(&pinctrl_mutex);
-
         return 0;
 }
 
diff --git a/drivers/pinctrl/pinctrl-bcm2835.c b/drivers/pinctrl/pinctrl-bcm2835.c
index a4adee633fa9..7e9be18ec2d2 100644
--- a/drivers/pinctrl/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/pinctrl-bcm2835.c
@@ -29,7 +29,6 @@
 #include <linux/irq.h>
 #include <linux/irqdesc.h>
 #include <linux/irqdomain.h>
-#include <linux/irq.h>
 #include <linux/module.h>
 #include <linux/of_address.h>
 #include <linux/of.h>
@@ -960,7 +959,7 @@ static int __devinit bcm2835_pinctrl_probe(struct platform_device *pdev)
                 return err;
         }
 
-        pc->base = devm_request_and_ioremap(&pdev->dev, &iomem);
+        pc->base = devm_request_and_ioremap(dev, &iomem);
         if (!pc->base)
                 return -EADDRNOTAVAIL;
 
@@ -1032,7 +1031,7 @@ static int __devinit bcm2835_pinctrl_probe(struct platform_device *pdev)
         pc->pctl_dev = pinctrl_register(&bcm2835_pinctrl_desc, dev, pc);
         if (!pc->pctl_dev) {
                 gpiochip_remove(&pc->gpio_chip);
-                return PTR_ERR(pc->pctl_dev);
+                return -EINVAL;
         }
 
         pc->gpio_range = bcm2835_pinctrl_gpio_range;
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8500.c b/drivers/pinctrl/pinctrl-nomadik-db8500.c
index ec6209dd7c39..debaa75b0552 100644
--- a/drivers/pinctrl/pinctrl-nomadik-db8500.c
+++ b/drivers/pinctrl/pinctrl-nomadik-db8500.c
@@ -725,10 +725,10 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
         DB8500_PIN_GROUP(spi0_c_1, NMK_GPIO_ALT_C),
         DB8500_PIN_GROUP(usbsim_c_2, NMK_GPIO_ALT_C),
         DB8500_PIN_GROUP(i2c3_c_2, NMK_GPIO_ALT_C),
-        /* Other alt C1 column, these are still configured as alt C */
-        DB8500_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C),
-        DB8500_PIN_GROUP(spi2_oc1_1, NMK_GPIO_ALT_C),
-        DB8500_PIN_GROUP(spi2_oc1_2, NMK_GPIO_ALT_C),
+        /* Other alt C1 column */
+        DB8500_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C1),
+        DB8500_PIN_GROUP(spi2_oc1_1, NMK_GPIO_ALT_C1),
+        DB8500_PIN_GROUP(spi2_oc1_2, NMK_GPIO_ALT_C1),
 };
 
 /* We use this macro to define the groups applicable to a function */
@@ -860,6 +860,284 @@ static const struct nmk_function nmk_db8500_functions[] = {
         FUNCTION(spi2),
 };
 
+static const struct prcm_gpiocr_altcx_pin_desc db8500_altcx_pins[] = {
+        PRCM_GPIOCR_ALTCX(23,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_CLK_a */
+                                true, PRCM_IDX_GPIOCR1, 7,      /* SBAG_CLK_a */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(24,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE or U2_RXD ??? */
+                                true, PRCM_IDX_GPIOCR1, 7,      /* SBAG_VAL_a */
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STM_MOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(25,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[0] */
+                                true, PRCM_IDX_GPIOCR1, 7,      /* SBAG_D_a[0] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(26,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[1] */
+                                true, PRCM_IDX_GPIOCR1, 7,      /* SBAG_D_a[1] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(27,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[2] */
+                                true, PRCM_IDX_GPIOCR1, 7,      /* SBAG_D_a[2] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(28,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[3] */
+                                true, PRCM_IDX_GPIOCR1, 7,      /* SBAG_D_a[3] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(29,   false, 0, 0,
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STM_MOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(30,   false, 0, 0,
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STM_MOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(31,   false, 0, 0,
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STM_MOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(32,   false, 0, 0,
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STM_MOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(68,   true, PRCM_IDX_GPIOCR1, 18,     /* REMAP_SELECT_ON */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(69,   true, PRCM_IDX_GPIOCR1, 18,     /* REMAP_SELECT_ON */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(70,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D23 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 11,     /* STM_MOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 8       /* SBAG_CLK */
+        ),
+        PRCM_GPIOCR_ALTCX(71,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D22 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 11,     /* STM_MOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 8       /* SBAG_D3 */
+        ),
+        PRCM_GPIOCR_ALTCX(72,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D21 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 11,     /* STM_MOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 8       /* SBAG_D2 */
+        ),
+        PRCM_GPIOCR_ALTCX(73,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D20 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 11,     /* STM_MOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 8       /* SBAG_D1 */
+        ),
+        PRCM_GPIOCR_ALTCX(74,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D19 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 11,     /* STM_MOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 8       /* SBAG_D0 */
+        ),
+        PRCM_GPIOCR_ALTCX(75,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D18 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 0,      /* DBG_UARTMOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(76,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D17 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                true, PRCM_IDX_GPIOCR1, 0,      /* DBG_UARTMOD_CMD0 */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(77,   true, PRCM_IDX_GPIOCR1, 5,      /* PTM_A9_D16 */
+                                true, PRCM_IDX_GPIOCR2, 2,      /* DBG_ETM_R4_CMD0 */
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 8       /* SBAG_VAL */
+        ),
+        PRCM_GPIOCR_ALTCX(86,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_O3 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(87,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_O2 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(88,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_I3 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(89,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_I2 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(90,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_O1 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(91,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_O0 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(92,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_I1 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(93,   true, PRCM_IDX_GPIOCR1, 12,     /* KP_I0 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(96,   true, PRCM_IDX_GPIOCR2, 3,      /* RF_INT */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(97,   true, PRCM_IDX_GPIOCR2, 1,      /* RF_CTRL */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(151,  false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_CTL */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS17 */
+        ),
+        PRCM_GPIOCR_ALTCX(152,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_CLK */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_CLK */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS16 */
+        ),
+        PRCM_GPIOCR_ALTCX(153,  true, PRCM_IDX_GPIOCR1, 1,      /* UARTMOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D15 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS15 */
+        ),
+        PRCM_GPIOCR_ALTCX(154,  true, PRCM_IDX_GPIOCR1, 1,      /* UARTMOD_CMD1 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D14 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS14 */
+        ),
+        PRCM_GPIOCR_ALTCX(155,  true, PRCM_IDX_GPIOCR1, 13,     /* STM_MOD_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D13 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS13 */
+        ),
+        PRCM_GPIOCR_ALTCX(156,  true, PRCM_IDX_GPIOCR1, 13,     /* STM_MOD_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D12 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS12 */
+        ),
+        PRCM_GPIOCR_ALTCX(157,  true, PRCM_IDX_GPIOCR1, 13,     /* STM_MOD_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D11 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS11 */
+        ),
+        PRCM_GPIOCR_ALTCX(158,  true, PRCM_IDX_GPIOCR1, 13,     /* STM_MOD_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D10 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS10 */
+        ),
+        PRCM_GPIOCR_ALTCX(159,  true, PRCM_IDX_GPIOCR1, 13,     /* STM_MOD_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D9 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS9 */
+        ),
+        PRCM_GPIOCR_ALTCX(160,  false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 14,     /* PTM_A9_D8 */
+                                true, PRCM_IDX_GPIOCR1, 19,     /* DBG_ETM_R4_CMD2 */
+                                true, PRCM_IDX_GPIOCR1, 25      /* HW_OBS8 */
+        ),
+        PRCM_GPIOCR_ALTCX(161,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO7 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D7 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS7 */
+        ),
+        PRCM_GPIOCR_ALTCX(162,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO6 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D6 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS6 */
+        ),
+        PRCM_GPIOCR_ALTCX(163,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO5 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D5 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS5 */
+        ),
+        PRCM_GPIOCR_ALTCX(164,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO4 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D4 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS4 */
+        ),
+        PRCM_GPIOCR_ALTCX(165,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO3 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D3 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS3 */
+        ),
+        PRCM_GPIOCR_ALTCX(166,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO2 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D2 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS2 */
+        ),
+        PRCM_GPIOCR_ALTCX(167,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO1 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D1 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS1 */
+        ),
+        PRCM_GPIOCR_ALTCX(168,  true, PRCM_IDX_GPIOCR1, 4,      /* Hx_GPIO0 */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* PTM_A9_D0 */
+                                true, PRCM_IDX_GPIOCR1, 15,     /* DBG_ETM_R4_CMD1*/
+                                true, PRCM_IDX_GPIOCR1, 24      /* HW_OBS0 */
+        ),
+        PRCM_GPIOCR_ALTCX(170,  true, PRCM_IDX_GPIOCR2, 2,      /* RF_INT */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(171,  true, PRCM_IDX_GPIOCR2, 0,      /* RF_CTRL */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(215,  true, PRCM_IDX_GPIOCR1, 23,     /* SPI2_TXD */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(216,  true, PRCM_IDX_GPIOCR1, 23,     /* SPI2_FRM */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(217,  true, PRCM_IDX_GPIOCR1, 23,     /* SPI2_CLK */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(218,  true, PRCM_IDX_GPIOCR1, 23,     /* SPI2_RXD */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+};
+
+static const u16 db8500_prcm_gpiocr_regs[] = {
+        [PRCM_IDX_GPIOCR1] = 0x138,
+        [PRCM_IDX_GPIOCR2] = 0x574,
+};
+
 static const struct nmk_pinctrl_soc_data nmk_db8500_soc = {
         .gpio_ranges = nmk_db8500_ranges,
         .gpio_num_ranges = ARRAY_SIZE(nmk_db8500_ranges),
@@ -869,6 +1147,9 @@ static const struct nmk_pinctrl_soc_data nmk_db8500_soc = {
         .nfunctions = ARRAY_SIZE(nmk_db8500_functions),
         .groups = nmk_db8500_groups,
         .ngroups = ARRAY_SIZE(nmk_db8500_groups),
+        .altcx_pins = db8500_altcx_pins,
+        .npins_altcx = ARRAY_SIZE(db8500_altcx_pins),
+        .prcm_gpiocr_registers = db8500_prcm_gpiocr_regs,
 };
 
 void __devinit
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8540.c b/drivers/pinctrl/pinctrl-nomadik-db8540.c
index 3daf665c84c3..52fc30181f7e 100644
--- a/drivers/pinctrl/pinctrl-nomadik-db8540.c
+++ b/drivers/pinctrl/pinctrl-nomadik-db8540.c
@@ -778,50 +778,50 @@ static const struct nmk_pingroup nmk_db8540_groups[] = {
         DB8540_PIN_GROUP(spi0_c_1, NMK_GPIO_ALT_C),
         DB8540_PIN_GROUP(i2c3_c_1, NMK_GPIO_ALT_C),
 
-        /* Other alt C1 column, these are still configured as alt C */
-        DB8540_PIN_GROUP(spi3_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(stmape_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(u2_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(remap0_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(remap1_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modobsrefclk_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modobspwrctrl_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modobsclkout_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(moduart1_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modprcmudbg_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modobsresout_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modaccgpo_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modxmip_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(i2c6_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(u2txrx_oc1_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(u2ctsrts_oc1_1, NMK_GPIO_ALT_C),
+        /* Other alt C1 column */
+        DB8540_PIN_GROUP(spi3_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(stmape_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(u2_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(remap0_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(remap1_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modobsrefclk_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modobspwrctrl_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modobsclkout_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(moduart1_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modprcmudbg_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modobsresout_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modaccgpo_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(modxmip_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(i2c6_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(u2txrx_oc1_1, NMK_GPIO_ALT_C1),
+        DB8540_PIN_GROUP(u2ctsrts_oc1_1, NMK_GPIO_ALT_C1),
 
-        /* Other alt C2 column, these are still configured as alt C */
-        DB8540_PIN_GROUP(sbag_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(hxclk_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modaccuart_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(stmmod_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(moduartstmmux_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(hxgpio_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(sbag_oc2_2, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modobsservice_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(moduart0_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(stmape_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(u2_oc2_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modxmip_oc2_1, NMK_GPIO_ALT_C),
+        /* Other alt C2 column */
+        DB8540_PIN_GROUP(sbag_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(hxclk_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(modaccuart_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(stmmod_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(moduartstmmux_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(hxgpio_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(sbag_oc2_2, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(modobsservice_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(moduart0_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(stmape_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(u2_oc2_1, NMK_GPIO_ALT_C2),
+        DB8540_PIN_GROUP(modxmip_oc2_1, NMK_GPIO_ALT_C2),
 
-        /* Other alt C3 column, these are still configured as alt C */
-        DB8540_PIN_GROUP(modaccgpo_oc3_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(tpui_oc3_1, NMK_GPIO_ALT_C),
+        /* Other alt C3 column */
+        DB8540_PIN_GROUP(modaccgpo_oc3_1, NMK_GPIO_ALT_C3),
+        DB8540_PIN_GROUP(tpui_oc3_1, NMK_GPIO_ALT_C3),
 
-        /* Other alt C4 column, these are still configured as alt C */
-        DB8540_PIN_GROUP(hwobs_oc4_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(moduart1txrx_oc4_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(moduart1rtscts_oc4_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modaccuarttxrx_oc4_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(modaccuartrtscts_oc4_1, NMK_GPIO_ALT_C),
-        DB8540_PIN_GROUP(stmmod_oc4_1, NMK_GPIO_ALT_C),
+        /* Other alt C4 column */
+        DB8540_PIN_GROUP(hwobs_oc4_1, NMK_GPIO_ALT_C4),
+        DB8540_PIN_GROUP(moduart1txrx_oc4_1, NMK_GPIO_ALT_C4),
+        DB8540_PIN_GROUP(moduart1rtscts_oc4_1, NMK_GPIO_ALT_C4),
+        DB8540_PIN_GROUP(modaccuarttxrx_oc4_1, NMK_GPIO_ALT_C4),
+        DB8540_PIN_GROUP(modaccuartrtscts_oc4_1, NMK_GPIO_ALT_C4),
+        DB8540_PIN_GROUP(stmmod_oc4_1, NMK_GPIO_ALT_C4),
 
 };
 
@@ -981,6 +981,265 @@ static const struct nmk_function nmk_db8540_functions[] = {
         FUNCTION(usb)
 };
 
+static const struct prcm_gpiocr_altcx_pin_desc db8540_altcx_pins[] = {
+        PRCM_GPIOCR_ALTCX(8,    true, PRCM_IDX_GPIOCR1, 20,     /* SPI3_CLK */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(9,    true, PRCM_IDX_GPIOCR1, 20,     /* SPI3_RXD */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(10,   true, PRCM_IDX_GPIOCR1, 20,     /* SPI3_FRM */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(11,   true, PRCM_IDX_GPIOCR1, 20,     /* SPI3_TXD */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(23,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_CLK_a */
+                                true, PRCM_IDX_GPIOCR2, 10,     /* SBAG_CLK_a */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(24,   true, PRCM_IDX_GPIOCR3, 30,     /* U2_RXD_g */
+                                true, PRCM_IDX_GPIOCR2, 10,     /* SBAG_VAL_a */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(25,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[0] */
+                                true, PRCM_IDX_GPIOCR2, 10,     /* SBAG_D_a[0] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(26,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[1] */
+                                true, PRCM_IDX_GPIOCR2, 10,     /* SBAG_D_a[1] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(27,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[2] */
+                                true, PRCM_IDX_GPIOCR2, 10,     /* SBAG_D_a[2] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(28,   true, PRCM_IDX_GPIOCR1, 9,      /* STMAPE_DAT_a[3] */
+                                true, PRCM_IDX_GPIOCR2, 10,     /* SBAG_D_a[3] */
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(64,   true, PRCM_IDX_GPIOCR1, 15,     /* MODOBS_REFCLK_REQ */
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_CTL */
+                                true, PRCM_IDX_GPIOCR2, 23      /* HW_OBS_APE_PRCMU[17] */
+        ),
+        PRCM_GPIOCR_ALTCX(65,   true, PRCM_IDX_GPIOCR1, 19,     /* MODOBS_PWRCTRL0 */
+                                true, PRCM_IDX_GPIOCR1, 24,     /* Hx_CLK */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_CLK */
+                                true, PRCM_IDX_GPIOCR2, 24      /* HW_OBS_APE_PRCMU[16] */
+        ),
+        PRCM_GPIOCR_ALTCX(66,   true, PRCM_IDX_GPIOCR1, 15,     /* MODOBS_CLKOUT1 */
+                                false, 0, 0,
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[15] */
+                                true, PRCM_IDX_GPIOCR2, 25      /* HW_OBS_APE_PRCMU[15] */
+        ),
+        PRCM_GPIOCR_ALTCX(67,   true, PRCM_IDX_GPIOCR1, 1,      /* MODUART1_TXD_a */
+                                true, PRCM_IDX_GPIOCR1, 6,      /* MODACCUART_TXD_a */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[14] */
+                                true, PRCM_IDX_GPIOCR2, 26      /* HW_OBS_APE_PRCMU[14] */
+        ),
+        PRCM_GPIOCR_ALTCX(70,   true, PRCM_IDX_GPIOCR3, 6,      /* MOD_PRCMU_DEBUG[17] */
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STMMOD_CLK_b */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[13] */
+                                true, PRCM_IDX_GPIOCR2, 27      /* HW_OBS_APE_PRCMU[13] */
+        ),
+        PRCM_GPIOCR_ALTCX(71,   true, PRCM_IDX_GPIOCR3, 6,      /* MOD_PRCMU_DEBUG[16] */
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STMMOD_DAT_b[3] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[12] */
+                                true, PRCM_IDX_GPIOCR2, 27      /* HW_OBS_APE_PRCMU[12] */
+        ),
+        PRCM_GPIOCR_ALTCX(72,   true, PRCM_IDX_GPIOCR3, 6,      /* MOD_PRCMU_DEBUG[15] */
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STMMOD_DAT_b[2] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[11] */
+                                true, PRCM_IDX_GPIOCR2, 27      /* HW_OBS_APE_PRCMU[11] */
+        ),
+        PRCM_GPIOCR_ALTCX(73,   true, PRCM_IDX_GPIOCR3, 6,      /* MOD_PRCMU_DEBUG[14] */
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STMMOD_DAT_b[1] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[10] */
+                                true, PRCM_IDX_GPIOCR2, 27      /* HW_OBS_APE_PRCMU[10] */
+        ),
+        PRCM_GPIOCR_ALTCX(74,   true, PRCM_IDX_GPIOCR3, 6,      /* MOD_PRCMU_DEBUG[13] */
+                                true, PRCM_IDX_GPIOCR1, 10,     /* STMMOD_DAT_b[0] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[9] */
+                                true, PRCM_IDX_GPIOCR2, 27      /* HW_OBS_APE_PRCMU[9] */
+        ),
+        PRCM_GPIOCR_ALTCX(75,   true, PRCM_IDX_GPIOCR1, 12,     /* MODOBS_RESOUT0_N */
+                                true, PRCM_IDX_GPIOCR2, 1,      /* MODUART_STMMUX_RXD_b */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[8] */
+                                true, PRCM_IDX_GPIOCR2, 28      /* HW_OBS_APE_PRCMU[8] */
+        ),
+        PRCM_GPIOCR_ALTCX(76,   true, PRCM_IDX_GPIOCR3, 7,      /* MOD_PRCMU_DEBUG[12] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[7] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[7] */
+                                true, PRCM_IDX_GPIOCR2, 29      /* HW_OBS_APE_PRCMU[7] */
+        ),
+        PRCM_GPIOCR_ALTCX(77,   true, PRCM_IDX_GPIOCR3, 7,      /* MOD_PRCMU_DEBUG[11] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[6] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[6] */
+                                true, PRCM_IDX_GPIOCR2, 29      /* HW_OBS_APE_PRCMU[6] */
+        ),
+        PRCM_GPIOCR_ALTCX(78,   true, PRCM_IDX_GPIOCR3, 7,      /* MOD_PRCMU_DEBUG[10] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[5] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[5] */
+                                true, PRCM_IDX_GPIOCR2, 29      /* HW_OBS_APE_PRCMU[5] */
+        ),
+        PRCM_GPIOCR_ALTCX(79,   true, PRCM_IDX_GPIOCR3, 7,      /* MOD_PRCMU_DEBUG[9] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[4] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[4] */
+                                true, PRCM_IDX_GPIOCR2, 29      /* HW_OBS_APE_PRCMU[4] */
+        ),
+        PRCM_GPIOCR_ALTCX(80,   true, PRCM_IDX_GPIOCR1, 26,     /* MODACC_GPO[0] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[3] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[3] */
+                                true, PRCM_IDX_GPIOCR2, 30      /* HW_OBS_APE_PRCMU[3] */
+        ),
+        PRCM_GPIOCR_ALTCX(81,   true, PRCM_IDX_GPIOCR2, 17,     /* MODACC_GPO[1] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[2] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[2] */
+                                true, PRCM_IDX_GPIOCR2, 30      /* HW_OBS_APE_PRCMU[2] */
+        ),
+        PRCM_GPIOCR_ALTCX(82,   true, PRCM_IDX_GPIOCR3, 8,      /* MOD_PRCMU_DEBUG[8] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[1] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[1] */
+                                true, PRCM_IDX_GPIOCR2, 31      /* HW_OBS_APE_PRCMU[1] */
+        ),
+        PRCM_GPIOCR_ALTCX(83,   true, PRCM_IDX_GPIOCR3, 8,      /* MOD_PRCMU_DEBUG[7] */
+                                true, PRCM_IDX_GPIOCR1, 25,     /* Hx_GPIO[0] */
+                                true, PRCM_IDX_GPIOCR1, 2,      /* TPIU_D[0] */
+                                true, PRCM_IDX_GPIOCR2, 31      /* HW_OBS_APE_PRCMU[0] */
+        ),
+        PRCM_GPIOCR_ALTCX(84,   true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[6] */
+                                true, PRCM_IDX_GPIOCR1, 8,      /* SBAG_CLK_b */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[23] */
+                                true, PRCM_IDX_GPIOCR1, 16      /* MODUART1_RXD_b */
+        ),
+        PRCM_GPIOCR_ALTCX(85,   true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[5] */
+                                true, PRCM_IDX_GPIOCR1, 8,      /* SBAG_D_b[3] */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[22] */
+                                true, PRCM_IDX_GPIOCR1, 16      /* MODUART1_TXD_b */
+        ),
+        PRCM_GPIOCR_ALTCX(86,   true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[0] */
+                                true, PRCM_IDX_GPIOCR2, 18,     /* STMAPE_DAT_b[0] */
+                                true, PRCM_IDX_GPIOCR1, 14,     /* TPIU_D[25] */
+                                true, PRCM_IDX_GPIOCR1, 11      /* STMMOD_DAT_c[0] */
+        ),
+        PRCM_GPIOCR_ALTCX(87,   true, PRCM_IDX_GPIOCR3, 0,      /* MODACC_GPO_a[5] */
+                                true, PRCM_IDX_GPIOCR2, 3,      /* U2_RXD_c */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[24] */
+                                true, PRCM_IDX_GPIOCR1, 21      /* MODUART_STMMUX_RXD_c */
+        ),
+        PRCM_GPIOCR_ALTCX(151,  true, PRCM_IDX_GPIOCR1, 18,     /* REMAP0 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(152,  true, PRCM_IDX_GPIOCR1, 18,     /* REMAP1 */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(153,  true, PRCM_IDX_GPIOCR3, 2,      /* KP_O_b[6] */
+                                true, PRCM_IDX_GPIOCR1, 8,      /* SBAG_D_b[2] */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[21] */
+                                true, PRCM_IDX_GPIOCR1, 0       /* MODUART1_RTS */
+        ),
+        PRCM_GPIOCR_ALTCX(154,  true, PRCM_IDX_GPIOCR3, 2,      /* KP_I_b[6] */
+                                true, PRCM_IDX_GPIOCR1, 8,      /* SBAG_D_b[1] */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[20] */
+                                true, PRCM_IDX_GPIOCR1, 0       /* MODUART1_CTS */
+        ),
+        PRCM_GPIOCR_ALTCX(155,  true, PRCM_IDX_GPIOCR3, 3,      /* KP_O_b[5] */
+                                true, PRCM_IDX_GPIOCR1, 8,      /* SBAG_D_b[0] */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[19] */
+                                true, PRCM_IDX_GPIOCR1, 5       /* MODACCUART_RXD_c */
+        ),
+        PRCM_GPIOCR_ALTCX(156,  true, PRCM_IDX_GPIOCR3, 3,      /* KP_O_b[4] */
+                                true, PRCM_IDX_GPIOCR1, 8,      /* SBAG_VAL_b */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[18] */
+                                true, PRCM_IDX_GPIOCR1, 5       /* MODACCUART_TXD_b */
+        ),
+        PRCM_GPIOCR_ALTCX(157,  true, PRCM_IDX_GPIOCR3, 4,      /* KP_I_b[5] */
+                                true, PRCM_IDX_GPIOCR1, 23,     /* MODOBS_SERVICE_N */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[17] */
+                                true, PRCM_IDX_GPIOCR1, 14      /* MODACCUART_RTS */
+        ),
+        PRCM_GPIOCR_ALTCX(158,  true, PRCM_IDX_GPIOCR3, 4,      /* KP_I_b[4] */
+                                true, PRCM_IDX_GPIOCR2, 0,      /* U2_TXD_c */
+                                true, PRCM_IDX_GPIOCR1, 3,      /* TPIU_D[16] */
+                                true, PRCM_IDX_GPIOCR1, 14      /* MODACCUART_CTS */
+        ),
+        PRCM_GPIOCR_ALTCX(159,  true, PRCM_IDX_GPIOCR3, 5,      /* KP_O_b[3] */
+                                true, PRCM_IDX_GPIOCR3, 10,     /* MODUART0_RXD */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[31] */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(160,  true, PRCM_IDX_GPIOCR3, 5,      /* KP_I_b[3] */
+                                true, PRCM_IDX_GPIOCR3, 10,     /* MODUART0_TXD */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[30] */
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(161,  true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[4] */
+                                true, PRCM_IDX_GPIOCR2, 18,     /* STMAPE_CLK_b */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[29] */
+                                true, PRCM_IDX_GPIOCR1, 11      /* STMMOD_CLK_c */
+        ),
+        PRCM_GPIOCR_ALTCX(162,  true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[3] */
+                                true, PRCM_IDX_GPIOCR2, 18,     /* STMAPE_DAT_b[3] */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[28] */
+                                true, PRCM_IDX_GPIOCR1, 11      /* STMMOD_DAT_c[3] */
+        ),
+        PRCM_GPIOCR_ALTCX(163,  true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[2] */
+                                true, PRCM_IDX_GPIOCR2, 18,     /* STMAPE_DAT_b[2] */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[27] */
+                                true, PRCM_IDX_GPIOCR1, 11      /* STMMOD_DAT_c[2] */
+        ),
+        PRCM_GPIOCR_ALTCX(164,  true, PRCM_IDX_GPIOCR3, 9,      /* MOD_PRCMU_DEBUG[1] */
+                                true, PRCM_IDX_GPIOCR2, 18,     /* STMAPE_DAT_b[1] */
+                                true, PRCM_IDX_GPIOCR1, 4,      /* TPIU_D[26] */
+                                true, PRCM_IDX_GPIOCR1, 11      /* STMMOD_DAT_c[1] */
+        ),
+        PRCM_GPIOCR_ALTCX(204,  true, PRCM_IDX_GPIOCR2, 2,      /* U2_RXD_f */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(205,  true, PRCM_IDX_GPIOCR2, 2,      /* U2_TXD_f */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(206,  true, PRCM_IDX_GPIOCR2, 2,      /* U2_CTSn_b */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+        PRCM_GPIOCR_ALTCX(207,  true, PRCM_IDX_GPIOCR2, 2,      /* U2_RTSn_b */
+                                false, 0, 0,
+                                false, 0, 0,
+                                false, 0, 0
+        ),
+};
+
+static const u16 db8540_prcm_gpiocr_regs[] = {
+        [PRCM_IDX_GPIOCR1] = 0x138,
+        [PRCM_IDX_GPIOCR2] = 0x574,
+        [PRCM_IDX_GPIOCR3] = 0x2bc,
+};
+
 static const struct nmk_pinctrl_soc_data nmk_db8540_soc = {
         .gpio_ranges = nmk_db8540_ranges,
         .gpio_num_ranges = ARRAY_SIZE(nmk_db8540_ranges),
@@ -990,6 +1249,9 @@ static const struct nmk_pinctrl_soc_data nmk_db8540_soc = {
         .nfunctions = ARRAY_SIZE(nmk_db8540_functions),
         .groups = nmk_db8540_groups,
         .ngroups = ARRAY_SIZE(nmk_db8540_groups),
+        .altcx_pins = db8540_altcx_pins,
+        .npins_altcx = ARRAY_SIZE(db8540_altcx_pins),
+        .prcm_gpiocr_registers = db8540_prcm_gpiocr_regs,
 };
 
 void __devinit
diff --git a/drivers/pinctrl/pinctrl-nomadik.c b/drivers/pinctrl/pinctrl-nomadik.c
index 6030a513f3c4..cf82d9ce4dee 100644
--- a/drivers/pinctrl/pinctrl-nomadik.c
+++ b/drivers/pinctrl/pinctrl-nomadik.c
@@ -30,6 +30,20 @@
 #include <linux/pinctrl/pinconf.h>
 /* Since we request GPIOs from ourself */
 #include <linux/pinctrl/consumer.h>
+/*
+ * For the U8500 archs, use the PRCMU register interface, for the older
+ * Nomadik, provide some stubs. The functions using these will only be
+ * called on the U8500 series.
+ */
+#ifdef CONFIG_ARCH_U8500
+#include <linux/mfd/dbx500-prcmu.h>
+#else
+static inline u32 prcmu_read(unsigned int reg) {
+        return 0;
+}
+static inline void prcmu_write(unsigned int reg, u32 value) {}
+static inline void prcmu_write_masked(unsigned int reg, u32 mask, u32 value) {}
+#endif
 
 #include <asm/mach/irq.h>
 
@@ -237,6 +251,89 @@ nmk_gpio_disable_lazy_irq(struct nmk_gpio_chip *nmk_chip, unsigned offset)
         dev_dbg(nmk_chip->chip.dev, "%d: clearing interrupt mask\n", gpio);
 }
 
+static void nmk_prcm_altcx_set_mode(struct nmk_pinctrl *npct,
+        unsigned offset, unsigned alt_num)
+{
+        int i;
+        u16 reg;
+        u8 bit;
+        u8 alt_index;
+        const struct prcm_gpiocr_altcx_pin_desc *pin_desc;
+        const u16 *gpiocr_regs;
+
+        if (alt_num > PRCM_IDX_GPIOCR_ALTC_MAX) {
+                dev_err(npct->dev, "PRCM GPIOCR: alternate-C%i is invalid\n",
+                        alt_num);
+                return;
+        }
+
+        for (i = 0 ; i < npct->soc->npins_altcx ; i++) {
+                if (npct->soc->altcx_pins[i].pin == offset)
+                        break;
+        }
+        if (i == npct->soc->npins_altcx) {
+                dev_dbg(npct->dev, "PRCM GPIOCR: pin %i is not found\n",
+                        offset);
+                return;
+        }
+
+        pin_desc = npct->soc->altcx_pins + i;
+        gpiocr_regs = npct->soc->prcm_gpiocr_registers;
+
+        /*
+         * If alt_num is NULL, just clear current ALTCx selection
+         * to make sure we come back to a pure ALTC selection
+         */
+        if (!alt_num) {
+                for (i = 0 ; i < PRCM_IDX_GPIOCR_ALTC_MAX ; i++) {
+                        if (pin_desc->altcx[i].used == true) {
+                                reg = gpiocr_regs[pin_desc->altcx[i].reg_index];
+                                bit = pin_desc->altcx[i].control_bit;
+                                if (prcmu_read(reg) & BIT(bit)) {
+                                        prcmu_write_masked(reg, BIT(bit), 0);
+                                        dev_dbg(npct->dev,
+                                                "PRCM GPIOCR: pin %i: alternate-C%i has been disabled\n",
+                                                offset, i+1);
+                                }
+                        }
+                }
+                return;
+        }
+
+        alt_index = alt_num - 1;
+        if (pin_desc->altcx[alt_index].used == false) {
+                dev_warn(npct->dev,
+                        "PRCM GPIOCR: pin %i: alternate-C%i does not exist\n",
+                        offset, alt_num);
+                return;
+        }
+
+        /*
+         * Check if any other ALTCx functions are activated on this pin
+         * and disable it first.
+         */
+        for (i = 0 ; i < PRCM_IDX_GPIOCR_ALTC_MAX ; i++) {
+                if (i == alt_index)
+                        continue;
+                if (pin_desc->altcx[i].used == true) {
+                        reg = gpiocr_regs[pin_desc->altcx[i].reg_index];
+                        bit = pin_desc->altcx[i].control_bit;
+                        if (prcmu_read(reg) & BIT(bit)) {
+                                prcmu_write_masked(reg, BIT(bit), 0);
+                                dev_dbg(npct->dev,
+                                        "PRCM GPIOCR: pin %i: alternate-C%i has been disabled\n",
+                                        offset, i+1);
+                        }
+                }
+        }
+
+        reg = gpiocr_regs[pin_desc->altcx[alt_index].reg_index];
+        bit = pin_desc->altcx[alt_index].control_bit;
+        dev_dbg(npct->dev, "PRCM GPIOCR: pin %i: alternate-C%i has been selected\n",
+                offset, alt_index+1);
+        prcmu_write_masked(reg, BIT(bit), BIT(bit));
+}
+
 static void __nmk_config_pin(struct nmk_gpio_chip *nmk_chip, unsigned offset,
                              pin_cfg_t cfg, bool sleep, unsigned int *slpmregs)
 {
@@ -959,7 +1056,7 @@ static int nmk_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
         struct nmk_gpio_chip *nmk_chip =
                 container_of(chip, struct nmk_gpio_chip, chip);
 
-        return irq_find_mapping(nmk_chip->domain, offset);
+        return irq_create_mapping(nmk_chip->domain, offset);
 }
 
 #ifdef CONFIG_DEBUG_FS
@@ -1184,6 +1281,7 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
         struct clk *clk;
         int secondary_irq;
         void __iomem *base;
+        int irq_start = 0;
         int irq;
         int ret;
 
@@ -1287,9 +1385,11 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 
         platform_set_drvdata(dev, nmk_chip);
 
-        nmk_chip->domain = irq_domain_add_legacy(np, NMK_GPIO_PER_CHIP,
-                                                NOMADIK_GPIO_TO_IRQ(pdata->first_gpio),
-                                                0, &nmk_gpio_irq_simple_ops, nmk_chip);
+        if (!np)
+                irq_start = NOMADIK_GPIO_TO_IRQ(pdata->first_gpio);
+        nmk_chip->domain = irq_domain_add_simple(np,
+                                NMK_GPIO_PER_CHIP, irq_start,
+                                &nmk_gpio_irq_simple_ops, nmk_chip);
         if (!nmk_chip->domain) {
                 dev_err(&dev->dev, "failed to create irqdomain\n");
                 ret = -ENOSYS;
@@ -1441,7 +1541,7 @@ static int nmk_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
          * IOFORCE will switch *all* ports to their sleepmode setting to as
          * to avoid glitches. (Not just one port!)
          */
-        glitch = (g->altsetting == NMK_GPIO_ALT_C);
+        glitch = ((g->altsetting & NMK_GPIO_ALT_C) == NMK_GPIO_ALT_C);
 
         if (glitch) {
                 spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
@@ -1491,8 +1591,21 @@ static int nmk_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
                  */
                 nmk_gpio_disable_lazy_irq(nmk_chip, bit);
 
-                __nmk_gpio_set_mode_safe(nmk_chip, bit, g->altsetting, glitch);
+                __nmk_gpio_set_mode_safe(nmk_chip, bit,
+                        (g->altsetting & NMK_GPIO_ALT_C), glitch);
                 clk_disable(nmk_chip->clk);
+
+                /*
+                 * Call PRCM GPIOCR config function in case ALTC
+                 * has been selected:
+                 * - If selection is a ALTCx, some bits in PRCM GPIOCR registers
+                 *   must be set.
+                 * - If selection is pure ALTC and previous selection was ALTCx,
+                 *   then some bits in PRCM GPIOCR registers must be cleared.
+                 */
+                if ((g->altsetting & NMK_GPIO_ALT_C) == NMK_GPIO_ALT_C)
+                        nmk_prcm_altcx_set_mode(npct, g->pins[i],
+                                g->altsetting >> NMK_GPIO_ALT_CX_SHIFT);
         }
 
         /* When all pins are successfully reconfigured we get here */
diff --git a/drivers/pinctrl/pinctrl-nomadik.h b/drivers/pinctrl/pinctrl-nomadik.h
index 5c99f1c62dfd..eef316e979a0 100644
--- a/drivers/pinctrl/pinctrl-nomadik.h
+++ b/drivers/pinctrl/pinctrl-nomadik.h
@@ -8,6 +8,78 @@
 #define PINCTRL_NMK_DB8500      1
 #define PINCTRL_NMK_DB8540      2
 
+#define PRCM_GPIOCR_ALTCX(pin_num,\
+        altc1_used, altc1_ri, altc1_cb,\
+        altc2_used, altc2_ri, altc2_cb,\
+        altc3_used, altc3_ri, altc3_cb,\
+        altc4_used, altc4_ri, altc4_cb)\
+{\
+        .pin = pin_num,\
+        .altcx[PRCM_IDX_GPIOCR_ALTC1] = {\
+                .used = altc1_used,\
+                .reg_index = altc1_ri,\
+                .control_bit = altc1_cb\
+        },\
+        .altcx[PRCM_IDX_GPIOCR_ALTC2] = {\
+                .used = altc2_used,\
+                .reg_index = altc2_ri,\
+                .control_bit = altc2_cb\
+        },\
+        .altcx[PRCM_IDX_GPIOCR_ALTC3] = {\
+                .used = altc3_used,\
+                .reg_index = altc3_ri,\
+                .control_bit = altc3_cb\
+        },\
+        .altcx[PRCM_IDX_GPIOCR_ALTC4] = {\
+                .used = altc4_used,\
+                .reg_index = altc4_ri,\
+                .control_bit = altc4_cb\
+        },\
+}
+
+/**
+ * enum prcm_gpiocr_reg_index
+ * Used to reference an PRCM GPIOCR register address.
+ */
+enum prcm_gpiocr_reg_index {
+        PRCM_IDX_GPIOCR1,
+        PRCM_IDX_GPIOCR2,
+        PRCM_IDX_GPIOCR3
+};
+/**
+ * enum prcm_gpiocr_altcx_index
+ * Used to reference an Other alternate-C function.
+ */
+enum prcm_gpiocr_altcx_index {
+        PRCM_IDX_GPIOCR_ALTC1,
+        PRCM_IDX_GPIOCR_ALTC2,
+        PRCM_IDX_GPIOCR_ALTC3,
+        PRCM_IDX_GPIOCR_ALTC4,
+        PRCM_IDX_GPIOCR_ALTC_MAX,
+};
+
+/**
+ * struct prcm_gpio_altcx - Other alternate-C function
+ * @used: other alternate-C function availability
+ * @reg_index: PRCM GPIOCR register index used to control the function
+ * @control_bit: PRCM GPIOCR bit used to control the function
+ */
+struct prcm_gpiocr_altcx {
+        bool used:1;
+        u8 reg_index:2;
+        u8 control_bit:5;
+} __packed;
+
+/**
+ * struct prcm_gpio_altcx_pin_desc - Other alternate-C pin
+ * @pin: The pin number
+ * @altcx: array of other alternate-C[1-4] functions
+ */
+struct prcm_gpiocr_altcx_pin_desc {
+        unsigned short pin;
+        struct prcm_gpiocr_altcx altcx[PRCM_IDX_GPIOCR_ALTC_MAX];
+};
+
 /**
  * struct nmk_function - Nomadik pinctrl mux function
  * @name: The name of the function, exported to pinctrl core.
@@ -50,6 +122,9 @@ struct nmk_pingroup {
  * @nfunction:  The number of entries in @functions.
  * @groups:     An array describing all pin groups the pin SoC supports.
  * @ngroups:    The number of entries in @groups.
+ * @altcx_pins: The pins that support Other alternate-C function on this SoC
+ * @npins_altcx: The number of Other alternate-C pins
+ * @prcm_gpiocr_registers: The array of PRCM GPIOCR registers on this SoC
  */
 struct nmk_pinctrl_soc_data {
         struct pinctrl_gpio_range *gpio_ranges;
@@ -60,6 +135,9 @@ struct nmk_pinctrl_soc_data {
         unsigned nfunctions;
         const struct nmk_pingroup *groups;
         unsigned ngroups;
+        const struct prcm_gpiocr_altcx_pin_desc *altcx_pins;
+        unsigned npins_altcx;
+        const u16 *prcm_gpiocr_registers;
 };
 
 #ifdef CONFIG_PINCTRL_STN8815
diff --git a/drivers/pinctrl/pinctrl-samsung.c b/drivers/pinctrl/pinctrl-samsung.c
index dd108a94acf9..861cd5f04d5e 100644
--- a/drivers/pinctrl/pinctrl-samsung.c
+++ b/drivers/pinctrl/pinctrl-samsung.c
@@ -513,7 +513,7 @@ static int samsung_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
  * Parse the pin names listed in the 'samsung,pins' property and convert it
  * into a list of gpio numbers are create a pin group from it.
  */
-static int __init samsung_pinctrl_parse_dt_pins(struct platform_device *pdev,
+static int __devinit samsung_pinctrl_parse_dt_pins(struct platform_device *pdev,
                         struct device_node *cfg_np, struct pinctrl_desc *pctl,
                         unsigned int **pin_list, unsigned int *npins)
 {
@@ -560,7 +560,7 @@ static int __init samsung_pinctrl_parse_dt_pins(struct platform_device *pdev,
  * from device node of the pin-controller. A pin group is formed with all
  * the pins listed in the "samsung,pins" property.
  */
-static int __init samsung_pinctrl_parse_dt(struct platform_device *pdev,
+static int __devinit samsung_pinctrl_parse_dt(struct platform_device *pdev,
                                 struct samsung_pinctrl_drv_data *drvdata)
 {
         struct device *dev = &pdev->dev;
@@ -655,7 +655,7 @@ static int __init samsung_pinctrl_parse_dt(struct platform_device *pdev,
 }
 
 /* register the pinctrl interface with the pinctrl subsystem */
-static int __init samsung_pinctrl_register(struct platform_device *pdev,
+static int __devinit samsung_pinctrl_register(struct platform_device *pdev,
                                 struct samsung_pinctrl_drv_data *drvdata)
 {
         struct pinctrl_desc *ctrldesc = &drvdata->pctl;
@@ -729,7 +729,7 @@ static int __init samsung_pinctrl_register(struct platform_device *pdev,
 }
 
 /* register the gpiolib interface with the gpiolib subsystem */
-static int __init samsung_gpiolib_register(struct platform_device *pdev,
+static int __devinit samsung_gpiolib_register(struct platform_device *pdev,
                                 struct samsung_pinctrl_drv_data *drvdata)
 {
         struct gpio_chip *gc;
@@ -762,7 +762,7 @@ static int __init samsung_gpiolib_register(struct platform_device *pdev,
 }
 
 /* unregister the gpiolib interface with the gpiolib subsystem */
-static int __init samsung_gpiolib_unregister(struct platform_device *pdev,
+static int __devinit samsung_gpiolib_unregister(struct platform_device *pdev,
                                 struct samsung_pinctrl_drv_data *drvdata)
 {
         int ret = gpiochip_remove(drvdata->gc);
diff --git a/drivers/pinctrl/pinctrl-sirf.c b/drivers/pinctrl/pinctrl-sirf.c
index 675497c15149..9ecacf3d0a75 100644
--- a/drivers/pinctrl/pinctrl-sirf.c
+++ b/drivers/pinctrl/pinctrl-sirf.c
@@ -1323,41 +1323,6 @@ static inline struct sirfsoc_gpio_bank *sirfsoc_gpio_to_bank(unsigned int gpio)
         return &sgpio_bank[gpio / SIRFSOC_GPIO_BANK_SIZE];
 }
 
-void sirfsoc_gpio_set_pull(unsigned gpio, unsigned mode)
-{
-        struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(gpio);
-        int idx = sirfsoc_gpio_to_offset(gpio);
-        u32 val, offset;
-        unsigned long flags;
-
-        offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
-
-        spin_lock_irqsave(&sgpio_lock, flags);
-
-        val = readl(bank->chip.regs + offset);
-
-        switch (mode) {
-        case SIRFSOC_GPIO_PULL_NONE:
-                val &= ~SIRFSOC_GPIO_CTL_PULL_MASK;
-                break;
-        case SIRFSOC_GPIO_PULL_UP:
-                val |= SIRFSOC_GPIO_CTL_PULL_MASK;
-                val |= SIRFSOC_GPIO_CTL_PULL_HIGH;
-                break;
-        case SIRFSOC_GPIO_PULL_DOWN:
-                val |= SIRFSOC_GPIO_CTL_PULL_MASK;
-                val &= ~SIRFSOC_GPIO_CTL_PULL_HIGH;
-                break;
-        default:
-                break;
-        }
-
-        writel(val, bank->chip.regs + offset);
-
-        spin_unlock_irqrestore(&sgpio_lock, flags);
-}
-EXPORT_SYMBOL(sirfsoc_gpio_set_pull);
-
 static inline struct sirfsoc_gpio_bank *sirfsoc_irqchip_to_bank(struct gpio_chip *chip)
 {
         return container_of(to_of_mm_gpio_chip(chip), struct sirfsoc_gpio_bank, chip);
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 729b686c3ad2..7da0b371fd65 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -464,7 +464,7 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
                 *bank = g->drv_bank;
                 *reg = g->drv_reg;
                 *bit = g->lpmd_bit;
-                *width = 1;
+                *width = 2;
                 break;
         case TEGRA_PINCONF_PARAM_DRIVE_DOWN_STRENGTH:
                 *bank = g->drv_bank;
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index 0386fdf0da16..7894f14c7059 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -3345,10 +3345,10 @@ static const struct tegra_function tegra30_functions[] = {
         FUNCTION(vi_alt3),
 };
 
-#define MUXCTL_REG_A    0x3000
-#define PINGROUP_REG_A  0x868
+#define DRV_PINGROUP_REG_A      0x868   /* bank 0 */
+#define PINGROUP_REG_A          0x3000  /* bank 1 */
 
-#define PINGROUP_REG_Y(r) ((r) - MUXCTL_REG_A)
+#define PINGROUP_REG_Y(r) ((r) - PINGROUP_REG_A)
 #define PINGROUP_REG_N(r) -1
 
 #define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior)   \
@@ -3364,25 +3364,25 @@ static const struct tegra_function tegra30_functions[] = {
                 },                                              \
                 .func_safe = TEGRA_MUX_ ## f_safe,              \
                 .mux_reg = PINGROUP_REG_Y(r),                   \
-                .mux_bank = 0,                                  \
+                .mux_bank = 1,                                  \
                 .mux_bit = 0,                                   \
                 .pupd_reg = PINGROUP_REG_Y(r),                  \
-                .pupd_bank = 0,                                 \
+                .pupd_bank = 1,                                 \
                 .pupd_bit = 2,                                  \
                 .tri_reg = PINGROUP_REG_Y(r),                   \
-                .tri_bank = 0,                                  \
+                .tri_bank = 1,                                  \
                 .tri_bit = 4,                                   \
                 .einput_reg = PINGROUP_REG_Y(r),                \
-                .einput_bank = 0,                               \
+                .einput_bank = 1,                               \
                 .einput_bit = 5,                                \
                 .odrain_reg = PINGROUP_REG_##od(r),             \
-                .odrain_bank = 0,                               \
+                .odrain_bank = 1,                               \
                 .odrain_bit = 6,                                \
                 .lock_reg = PINGROUP_REG_Y(r),                  \
-                .lock_bank = 0,                                 \
+                .lock_bank = 1,                                 \
                 .lock_bit = 7,                                  \
                 .ioreset_reg = PINGROUP_REG_##ior(r),           \
-                .ioreset_bank = 0,                              \
+                .ioreset_bank = 1,                              \
                 .ioreset_bit = 8,                               \
                 .drv_reg = -1,                                  \
         }
@@ -3401,8 +3401,8 @@ static const struct tegra_function tegra30_functions[] = {
                 .odrain_reg = -1,                               \
                 .lock_reg = -1,                                 \
                 .ioreset_reg = -1,                              \
-                .drv_reg = ((r) - PINGROUP_REG_A),              \
-                .drv_bank = 1,                                  \
+                .drv_reg = ((r) - DRV_PINGROUP_REG_A),          \
+                .drv_bank = 0,                                  \
                 .hsm_bit = hsm_b,                               \
                 .schmitt_bit = schmitt_b,                       \
                 .lpmd_bit = lpmd_b,                             \
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
index f8d917d40c92..b9bcaec66223 100644
--- a/drivers/pinctrl/pinctrl-xway.c
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -17,8 +17,6 @@
 #include <linux/ioport.h>
 #include <linux/io.h>
 #include <linux/device.h>
-#include <linux/module.h>
-#include <linux/io.h>
 #include <linux/platform_device.h>
 
 #include "pinctrl-lantiq.h"
diff --git a/drivers/platform/x86/acerhdf.c b/drivers/platform/x86/acerhdf.c
index 39abb150bdd4..84c56881ba80 100644
--- a/drivers/platform/x86/acerhdf.c
+++ b/drivers/platform/x86/acerhdf.c
@@ -329,7 +329,8 @@ static int acerhdf_bind(struct thermal_zone_device *thermal,
         if (cdev != cl_dev)
                 return 0;
 
-        if (thermal_zone_bind_cooling_device(thermal, 0, cdev)) {
+        if (thermal_zone_bind_cooling_device(thermal, 0, cdev,
+                        THERMAL_NO_LIMIT, THERMAL_NO_LIMIT)) {
                 pr_err("error binding cooling dev\n");
                 return -EINVAL;
         }
@@ -661,7 +662,7 @@ static int acerhdf_register_thermal(void)
                 return -EINVAL;
 
         thz_dev = thermal_zone_device_register("acerhdf", 1, 0, NULL,
-                                              &acerhdf_dev_ops, 0, 0, 0,
+                                              &acerhdf_dev_ops, 0,
                                               (kernelmode) ? interval*1000 : 0);
         if (IS_ERR(thz_dev))
                 return -EINVAL;
diff --git a/drivers/platform/x86/intel_mid_thermal.c b/drivers/platform/x86/intel_mid_thermal.c
index 3a27113deda9..c8097616dd62 100644
--- a/drivers/platform/x86/intel_mid_thermal.c
+++ b/drivers/platform/x86/intel_mid_thermal.c
@@ -502,7 +502,7 @@ static int mid_thermal_probe(struct platform_device *pdev)
                         goto err;
                 }
                 pinfo->tzd[i] = thermal_zone_device_register(name[i],
-                                0, 0, td_info, &tzd_ops, 0, 0, 0, 0);
+                                0, 0, td_info, &tzd_ops, 0, 0);
                 if (IS_ERR(pinfo->tzd[i])) {
                         kfree(td_info);
                         ret = PTR_ERR(pinfo->tzd[i]);
diff --git a/drivers/power/avs/smartreflex.c b/drivers/power/avs/smartreflex.c
index d4957b4edb62..24768a27e1d8 100644
--- a/drivers/power/avs/smartreflex.c
+++ b/drivers/power/avs/smartreflex.c
@@ -930,7 +930,7 @@ static int __init omap_sr_probe(struct platform_device *pdev)
         if (!sr_info->base) {
                 dev_err(&pdev->dev, "%s: ioremap fail\n", __func__);
                 ret = -ENOMEM;
-                goto err_release_region;
+                goto err_free_name;
         }
 
         if (irq)
@@ -969,7 +969,7 @@ static int __init omap_sr_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "%s: Unable to create debugfs directory\n",
                         __func__);
                 ret = PTR_ERR(sr_info->dbg_dir);
-                goto err_free_name;
+                goto err_debugfs;
         }
 
         (void) debugfs_create_file("autocomp", S_IRUGO | S_IWUSR,
@@ -1013,11 +1013,11 @@ static int __init omap_sr_probe(struct platform_device *pdev)
 
 err_debugfs:
         debugfs_remove_recursive(sr_info->dbg_dir);
-err_free_name:
-        kfree(sr_info->name);
 err_iounmap:
         list_del(&sr_info->node);
         iounmap(sr_info->base);
+err_free_name:
+        kfree(sr_info->name);
 err_release_region:
         release_mem_region(mem->start, resource_size(mem));
 err_free_devinfo:
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 08cc8a3c15af..2436f1350013 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -201,7 +201,7 @@ static int psy_register_thermal(struct power_supply *psy)
         for (i = 0; i < psy->num_properties; i++) {
                 if (psy->properties[i] == POWER_SUPPLY_PROP_TEMP) {
                         psy->tzd = thermal_zone_device_register(psy->name, 0, 0,
-                                        psy, &psy_tzd_ops, 0, 0, 0, 0);
+                                        psy, &psy_tzd_ops, 0, 0);
                         if (IS_ERR(psy->tzd))
                                 return PTR_ERR(psy->tzd);
                         break;
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index d7c6b83097c1..ed81720e7b2b 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -1,6 +1,5 @@
 menuconfig PWM
         bool "Pulse-Width Modulation (PWM) Support"
-        depends on !MACH_JZ4740 && !PUV3_PWM
         help
           Generic Pulse-Width Modulation (PWM) support.
 
@@ -29,6 +28,15 @@ menuconfig PWM
 
 if PWM
 
+config PWM_AB8500
+        tristate "AB8500 PWM support"
+        depends on AB8500_CORE && ARCH_U8500
+        help
+          Generic PWM framework driver for Analog Baseband AB8500.
+
+          To compile this driver as a module, choose M here: the module
+          will be called pwm-ab8500.
+
 config PWM_BFIN
         tristate "Blackfin PWM support"
         depends on BFIN_GPTIMERS
@@ -47,6 +55,16 @@ config PWM_IMX
           To compile this driver as a module, choose M here: the module
           will be called pwm-imx.
 
+config PWM_JZ4740
+        tristate "Ingenic JZ4740 PWM support"
+        depends on MACH_JZ4740
+        help
+          Generic PWM framework driver for Ingenic JZ4740 based
+          machines.
+
+          To compile this driver as a module, choose M here: the module
+          will be called pwm-jz4740.
+
 config PWM_LPC32XX
         tristate "LPC32XX PWM support"
         depends on ARCH_LPC32XX
@@ -67,6 +85,15 @@ config PWM_MXS
           To compile this driver as a module, choose M here: the module
           will be called pwm-mxs.
 
+config PWM_PUV3
+        tristate "PKUnity NetBook-0916 PWM support"
+        depends on ARCH_PUV3
+        help
+          Generic PWM framework driver for PKUnity NetBook-0916.
+
+          To compile this driver as a module, choose M here: the module
+          will be called pwm-puv3.
+
 config PWM_PXA
         tristate "PXA PWM support"
         depends on ARCH_PXA
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index 78f123dca30d..acfe4821c58b 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -1,8 +1,11 @@
 obj-$(CONFIG_PWM)               += core.o
+obj-$(CONFIG_PWM_AB8500)        += pwm-ab8500.o
 obj-$(CONFIG_PWM_BFIN)          += pwm-bfin.o
 obj-$(CONFIG_PWM_IMX)           += pwm-imx.o
+obj-$(CONFIG_PWM_JZ4740)        += pwm-jz4740.o
 obj-$(CONFIG_PWM_LPC32XX)       += pwm-lpc32xx.o
 obj-$(CONFIG_PWM_MXS)           += pwm-mxs.o
+obj-$(CONFIG_PWM_PUV3)          += pwm-puv3.o
 obj-$(CONFIG_PWM_PXA)           += pwm-pxa.o
 obj-$(CONFIG_PWM_SAMSUNG)       += pwm-samsung.o
 obj-$(CONFIG_PWM_TEGRA)         += pwm-tegra.o
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index c6e05078d3ad..f5acdaa52707 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -371,7 +371,7 @@ EXPORT_SYMBOL_GPL(pwm_free);
  */
 int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 {
-        if (!pwm || period_ns == 0 || duty_ns > period_ns)
+        if (!pwm || duty_ns < 0 || period_ns <= 0 || duty_ns > period_ns)
                 return -EINVAL;
 
         return pwm->chip->ops->config(pwm->chip, pwm, duty_ns, period_ns);
@@ -379,6 +379,28 @@ int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 EXPORT_SYMBOL_GPL(pwm_config);
 
 /**
+ * pwm_set_polarity() - configure the polarity of a PWM signal
+ * @pwm: PWM device
+ * @polarity: new polarity of the PWM signal
+ *
+ * Note that the polarity cannot be configured while the PWM device is enabled
+ */
+int pwm_set_polarity(struct pwm_device *pwm, enum pwm_polarity polarity)
+{
+        if (!pwm || !pwm->chip->ops)
+                return -EINVAL;
+
+        if (!pwm->chip->ops->set_polarity)
+                return -ENOSYS;
+
+        if (test_bit(PWMF_ENABLED, &pwm->flags))
+                return -EBUSY;
+
+        return pwm->chip->ops->set_polarity(pwm->chip, pwm, polarity);
+}
+EXPORT_SYMBOL_GPL(pwm_set_polarity);
+
+/**
  * pwm_enable() - start a PWM output toggling
  * @pwm: PWM device
  */
@@ -624,6 +646,64 @@ out:
 }
 EXPORT_SYMBOL_GPL(pwm_put);
 
+static void devm_pwm_release(struct device *dev, void *res)
+{
+        pwm_put(*(struct pwm_device **)res);
+}
+
+/**
+ * devm_pwm_get() - resource managed pwm_get()
+ * @dev: device for PWM consumer
+ * @con_id: consumer name
+ *
+ * This function performs like pwm_get() but the acquired PWM device will
+ * automatically be released on driver detach.
+ */
+struct pwm_device *devm_pwm_get(struct device *dev, const char *con_id)
+{
+        struct pwm_device **ptr, *pwm;
+
+        ptr = devres_alloc(devm_pwm_release, sizeof(**ptr), GFP_KERNEL);
+        if (!ptr)
+                return ERR_PTR(-ENOMEM);
+
+        pwm = pwm_get(dev, con_id);
+        if (!IS_ERR(pwm)) {
+                *ptr = pwm;
+                devres_add(dev, ptr);
+        } else {
+                devres_free(ptr);
+        }
+
+        return pwm;
+}
+EXPORT_SYMBOL_GPL(devm_pwm_get);
+
+static int devm_pwm_match(struct device *dev, void *res, void *data)
+{
+        struct pwm_device **p = res;
+
+        if (WARN_ON(!p || !*p))
+                return 0;
+
+        return *p == data;
+}
+
+/**
+ * devm_pwm_put() - resource managed pwm_put()
+ * @dev: device for PWM consumer
+ * @pwm: PWM device
+ *
+ * Release a PWM previously allocated using devm_pwm_get(). Calling this
+ * function is usually not needed because devm-allocated resources are
+ * automatically released on driver detach.
+ */
+void devm_pwm_put(struct device *dev, struct pwm_device *pwm)
+{
+        WARN_ON(devres_release(dev, devm_pwm_release, devm_pwm_match, pwm));
+}
+EXPORT_SYMBOL_GPL(devm_pwm_put);
+
 #ifdef CONFIG_DEBUG_FS
 static void pwm_dbg_show(struct pwm_chip *chip, struct seq_file *s)
 {
diff --git a/drivers/misc/ab8500-pwm.c b/drivers/pwm/pwm-ab8500.c
index d7a9aa14e5d5..cfb72ca873d1 100644
--- a/drivers/misc/ab8500-pwm.c
+++ b/drivers/pwm/pwm-ab8500.c
@@ -24,16 +24,12 @@
 #define ENABLE_PWM                      1
 #define DISABLE_PWM                     0
 
-struct pwm_device {
-        struct device *dev;
-        struct list_head node;
-        const char *label;
-        unsigned int pwm_id;
+struct ab8500_pwm_chip {
+        struct pwm_chip chip;
 };
 
-static LIST_HEAD(pwm_list);
-
-int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
+static int ab8500_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+                             int duty_ns, int period_ns)
 {
         int ret = 0;
         unsigned int higher_val, lower_val;
@@ -50,95 +46,94 @@ int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
          */
         higher_val = ((duty_ns & 0x0300) >> 8);
 
-        reg = AB8500_PWM_OUT_CTRL1_REG + ((pwm->pwm_id - 1) * 2);
+        reg = AB8500_PWM_OUT_CTRL1_REG + ((chip->base - 1) * 2);
 
-        ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+        ret = abx500_set_register_interruptible(chip->dev, AB8500_MISC,
                         reg, (u8)lower_val);
         if (ret < 0)
                 return ret;
-        ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+        ret = abx500_set_register_interruptible(chip->dev, AB8500_MISC,
                         (reg + 1), (u8)higher_val);
 
         return ret;
 }
-EXPORT_SYMBOL(pwm_config);
 
-int pwm_enable(struct pwm_device *pwm)
+static int ab8500_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
         int ret;
 
-        ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+        ret = abx500_mask_and_set_register_interruptible(chip->dev,
                                 AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-                                1 << (pwm->pwm_id-1), ENABLE_PWM);
+                                1 << (chip->base - 1), ENABLE_PWM);
         if (ret < 0)
-                dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+                dev_err(chip->dev, "%s: Failed to disable PWM, Error %d\n",
                                                         pwm->label, ret);
         return ret;
 }
-EXPORT_SYMBOL(pwm_enable);
 
-void pwm_disable(struct pwm_device *pwm)
+static void ab8500_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
         int ret;
 
-        ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+        ret = abx500_mask_and_set_register_interruptible(chip->dev,
                                 AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-                                1 << (pwm->pwm_id-1), DISABLE_PWM);
+                                1 << (chip->base - 1), DISABLE_PWM);
         if (ret < 0)
-                dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+                dev_err(chip->dev, "%s: Failed to disable PWM, Error %d\n",
                                                         pwm->label, ret);
         return;
 }
-EXPORT_SYMBOL(pwm_disable);
-
-struct pwm_device *pwm_request(int pwm_id, const char *label)
-{
-        struct pwm_device *pwm;
-
-        list_for_each_entry(pwm, &pwm_list, node) {
-                if (pwm->pwm_id == pwm_id) {
-                        pwm->label = label;
-                        pwm->pwm_id = pwm_id;
-                        return pwm;
-                }
-        }
-
-        return ERR_PTR(-ENOENT);
-}
-EXPORT_SYMBOL(pwm_request);
 
-void pwm_free(struct pwm_device *pwm)
-{
-        pwm_disable(pwm);
-}
-EXPORT_SYMBOL(pwm_free);
+static const struct pwm_ops ab8500_pwm_ops = {
+        .config = ab8500_pwm_config,
+        .enable = ab8500_pwm_enable,
+        .disable = ab8500_pwm_disable,
+};
 
 static int __devinit ab8500_pwm_probe(struct platform_device *pdev)
 {
-        struct pwm_device *pwm;
+        struct ab8500_pwm_chip *ab8500;
+        int err;
+
         /*
          * Nothing to be done in probe, this is required to get the
          * device which is required for ab8500 read and write
          */
-        pwm = kzalloc(sizeof(struct pwm_device), GFP_KERNEL);
-        if (pwm == NULL) {
+        ab8500 = kzalloc(sizeof(*ab8500), GFP_KERNEL);
+        if (ab8500 == NULL) {
                 dev_err(&pdev->dev, "failed to allocate memory\n");
                 return -ENOMEM;
         }
-        pwm->dev = &pdev->dev;
-        pwm->pwm_id = pdev->id;
-        list_add_tail(&pwm->node, &pwm_list);
-        platform_set_drvdata(pdev, pwm);
-        dev_dbg(pwm->dev, "pwm probe successful\n");
+
+        ab8500->chip.dev = &pdev->dev;
+        ab8500->chip.ops = &ab8500_pwm_ops;
+        ab8500->chip.base = pdev->id;
+        ab8500->chip.npwm = 1;
+
+        err = pwmchip_add(&ab8500->chip);
+        if (err < 0) {
+                kfree(ab8500);
+                return err;
+        }
+
+        dev_dbg(&pdev->dev, "pwm probe successful\n");
+        platform_set_drvdata(pdev, ab8500);
+
         return 0;
 }
 
 static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
 {
-        struct pwm_device *pwm = platform_get_drvdata(pdev);
-        list_del(&pwm->node);
+        struct ab8500_pwm_chip *ab8500 = platform_get_drvdata(pdev);
+        int err;
+
+        err = pwmchip_remove(&ab8500->chip);
+        if (err < 0)
+                return err;
+
         dev_dbg(&pdev->dev, "pwm driver removed\n");
-        kfree(pwm);
+        kfree(ab8500);
+
         return 0;
 }
 
@@ -150,19 +145,8 @@ static struct platform_driver ab8500_pwm_driver = {
         .probe = ab8500_pwm_probe,
         .remove = __devexit_p(ab8500_pwm_remove),
 };
+module_platform_driver(ab8500_pwm_driver);
 
-static int __init ab8500_pwm_init(void)
-{
-        return platform_driver_register(&ab8500_pwm_driver);
-}
-
-static void __exit ab8500_pwm_exit(void)
-{
-        platform_driver_unregister(&ab8500_pwm_driver);
-}
-
-subsys_initcall(ab8500_pwm_init);
-module_exit(ab8500_pwm_exit);
 MODULE_AUTHOR("Arun MURTHY <arun.murthy@stericsson.com>");
 MODULE_DESCRIPTION("AB8500 Pulse Width Modulation Driver");
 MODULE_ALIAS("platform:ab8500-pwm");
diff --git a/drivers/pwm/pwm-bfin.c b/drivers/pwm/pwm-bfin.c
index d53c4e7941ef..5da8e185e838 100644
--- a/drivers/pwm/pwm-bfin.c
+++ b/drivers/pwm/pwm-bfin.c
@@ -69,9 +69,6 @@ static int bfin_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         unsigned long period, duty;
         unsigned long long val;
 
-        if (duty_ns < 0 || duty_ns > period_ns)
-                return -EINVAL;
-
         val = (unsigned long long)get_sclk() * period_ns;
         do_div(val, NSEC_PER_SEC);
         period = val;
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index 2a0b35333972..8a5d3ae2946a 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -16,8 +16,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/pwm.h>
-#include <mach/hardware.h>
-
+#include <linux/of_device.h>
 
 /* i.MX1 and i.MX21 share the same PWM function block: */
 
@@ -25,6 +24,7 @@
 #define MX1_PWMS    0x04   /* PWM Sample Register */
 #define MX1_PWMP    0x08   /* PWM Period Register */
 
+#define MX1_PWMC_EN             (1 << 4)
 
 /* i.MX27, i.MX31, i.MX35 share the same PWM function block: */
 
@@ -40,110 +40,165 @@
 #define MX3_PWMCR_EN              (1 << 0)
 
 struct imx_chip {
-        struct clk      *clk;
+        struct clk      *clk_per;
+        struct clk      *clk_ipg;
 
-        int             clk_enabled;
+        int             enabled;
         void __iomem    *mmio_base;
 
         struct pwm_chip chip;
+
+        int (*config)(struct pwm_chip *chip,
+                struct pwm_device *pwm, int duty_ns, int period_ns);
+        void (*set_enable)(struct pwm_chip *chip, bool enable);
 };
 
 #define to_imx_chip(chip)       container_of(chip, struct imx_chip, chip)
 
-static int imx_pwm_config(struct pwm_chip *chip,
+static int imx_pwm_config_v1(struct pwm_chip *chip,
                 struct pwm_device *pwm, int duty_ns, int period_ns)
 {
         struct imx_chip *imx = to_imx_chip(chip);
 
-        if (!(cpu_is_mx1() || cpu_is_mx21())) {
-                unsigned long long c;
-                unsigned long period_cycles, duty_cycles, prescale;
-                u32 cr;
-
-                c = clk_get_rate(imx->clk);
-                c = c * period_ns;
-                do_div(c, 1000000000);
-                period_cycles = c;
-
-                prescale = period_cycles / 0x10000 + 1;
-
-                period_cycles /= prescale;
-                c = (unsigned long long)period_cycles * duty_ns;
-                do_div(c, period_ns);
-                duty_cycles = c;
-
-                /*
-                 * according to imx pwm RM, the real period value should be
-                 * PERIOD value in PWMPR plus 2.
-                 */
-                if (period_cycles > 2)
-                        period_cycles -= 2;
-                else
-                        period_cycles = 0;
-
-                writel(duty_cycles, imx->mmio_base + MX3_PWMSAR);
-                writel(period_cycles, imx->mmio_base + MX3_PWMPR);
-
-                cr = MX3_PWMCR_PRESCALER(prescale) |
-                        MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
-                        MX3_PWMCR_DBGEN | MX3_PWMCR_EN;
-
-                if (cpu_is_mx25())
-                        cr |= MX3_PWMCR_CLKSRC_IPG;
-                else
-                        cr |= MX3_PWMCR_CLKSRC_IPG_HIGH;
-
-                writel(cr, imx->mmio_base + MX3_PWMCR);
-        } else if (cpu_is_mx1() || cpu_is_mx21()) {
-                /* The PWM subsystem allows for exact frequencies. However,
-                 * I cannot connect a scope on my device to the PWM line and
-                 * thus cannot provide the program the PWM controller
-                 * exactly. Instead, I'm relying on the fact that the
-                 * Bootloader (u-boot or WinCE+haret) has programmed the PWM
-                 * function group already. So I'll just modify the PWM sample
-                 * register to follow the ratio of duty_ns vs. period_ns
-                 * accordingly.
-                 *
-                 * This is good enough for programming the brightness of
-                 * the LCD backlight.
-                 *
-                 * The real implementation would divide PERCLK[0] first by
-                 * both the prescaler (/1 .. /128) and then by CLKSEL
-                 * (/2 .. /16).
-                 */
-                u32 max = readl(imx->mmio_base + MX1_PWMP);
-                u32 p = max * duty_ns / period_ns;
-                writel(max - p, imx->mmio_base + MX1_PWMS);
-        } else {
-                BUG();
-        }
+        /*
+         * The PWM subsystem allows for exact frequencies. However,
+         * I cannot connect a scope on my device to the PWM line and
+         * thus cannot provide the program the PWM controller
+         * exactly. Instead, I'm relying on the fact that the
+         * Bootloader (u-boot or WinCE+haret) has programmed the PWM
+         * function group already. So I'll just modify the PWM sample
+         * register to follow the ratio of duty_ns vs. period_ns
+         * accordingly.
+         *
+         * This is good enough for programming the brightness of
+         * the LCD backlight.
+         *
+         * The real implementation would divide PERCLK[0] first by
+         * both the prescaler (/1 .. /128) and then by CLKSEL
+         * (/2 .. /16).
+         */
+        u32 max = readl(imx->mmio_base + MX1_PWMP);
+        u32 p = max * duty_ns / period_ns;
+        writel(max - p, imx->mmio_base + MX1_PWMS);
 
         return 0;
 }
 
+static void imx_pwm_set_enable_v1(struct pwm_chip *chip, bool enable)
+{
+        struct imx_chip *imx = to_imx_chip(chip);
+        u32 val;
+
+        val = readl(imx->mmio_base + MX1_PWMC);
+
+        if (enable)
+                val |= MX1_PWMC_EN;
+        else
+                val &= ~MX1_PWMC_EN;
+
+        writel(val, imx->mmio_base + MX1_PWMC);
+}
+
+static int imx_pwm_config_v2(struct pwm_chip *chip,
+                struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+        struct imx_chip *imx = to_imx_chip(chip);
+        unsigned long long c;
+        unsigned long period_cycles, duty_cycles, prescale;
+        u32 cr;
+
+        c = clk_get_rate(imx->clk_per);
+        c = c * period_ns;
+        do_div(c, 1000000000);
+        period_cycles = c;
+
+        prescale = period_cycles / 0x10000 + 1;
+
+        period_cycles /= prescale;
+        c = (unsigned long long)period_cycles * duty_ns;
+        do_div(c, period_ns);
+        duty_cycles = c;
+
+        /*
+         * according to imx pwm RM, the real period value should be
+         * PERIOD value in PWMPR plus 2.
+         */
+        if (period_cycles > 2)
+                period_cycles -= 2;
+        else
+                period_cycles = 0;
+
+        writel(duty_cycles, imx->mmio_base + MX3_PWMSAR);
+        writel(period_cycles, imx->mmio_base + MX3_PWMPR);
+
+        cr = MX3_PWMCR_PRESCALER(prescale) |
+                MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
+                MX3_PWMCR_DBGEN | MX3_PWMCR_CLKSRC_IPG_HIGH;
+
+        if (imx->enabled)
+                cr |= MX3_PWMCR_EN;
+
+        writel(cr, imx->mmio_base + MX3_PWMCR);
+
+        return 0;
+}
+
+static void imx_pwm_set_enable_v2(struct pwm_chip *chip, bool enable)
+{
+        struct imx_chip *imx = to_imx_chip(chip);
+        u32 val;
+
+        val = readl(imx->mmio_base + MX3_PWMCR);
+
+        if (enable)
+                val |= MX3_PWMCR_EN;
+        else
+                val &= ~MX3_PWMCR_EN;
+
+        writel(val, imx->mmio_base + MX3_PWMCR);
+}
+
+static int imx_pwm_config(struct pwm_chip *chip,
+                struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+        struct imx_chip *imx = to_imx_chip(chip);
+        int ret;
+
+        ret = clk_prepare_enable(imx->clk_ipg);
+        if (ret)
+                return ret;
+
+        ret = imx->config(chip, pwm, duty_ns, period_ns);
+
+        clk_disable_unprepare(imx->clk_ipg);
+
+        return ret;
+}
+
 static int imx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
         struct imx_chip *imx = to_imx_chip(chip);
-        int rc = 0;
+        int ret;
 
-        if (!imx->clk_enabled) {
-                rc = clk_prepare_enable(imx->clk);
-                if (!rc)
-                        imx->clk_enabled = 1;
-        }
-        return rc;
+        ret = clk_prepare_enable(imx->clk_per);
+        if (ret)
+                return ret;
+
+        imx->set_enable(chip, true);
+
+        imx->enabled = 1;
+
+        return 0;
 }
 
 static void imx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
         struct imx_chip *imx = to_imx_chip(chip);
 
-        writel(0, imx->mmio_base + MX3_PWMCR);
+        imx->set_enable(chip, false);
 
-        if (imx->clk_enabled) {
-                clk_disable_unprepare(imx->clk);
-                imx->clk_enabled = 0;
-        }
+        clk_disable_unprepare(imx->clk_per);
+        imx->enabled = 0;
 }
 
 static struct pwm_ops imx_pwm_ops = {
@@ -153,30 +208,66 @@ static struct pwm_ops imx_pwm_ops = {
         .owner = THIS_MODULE,
 };
 
+struct imx_pwm_data {
+        int (*config)(struct pwm_chip *chip,
+                struct pwm_device *pwm, int duty_ns, int period_ns);
+        void (*set_enable)(struct pwm_chip *chip, bool enable);
+};
+
+static struct imx_pwm_data imx_pwm_data_v1 = {
+        .config = imx_pwm_config_v1,
+        .set_enable = imx_pwm_set_enable_v1,
+};
+
+static struct imx_pwm_data imx_pwm_data_v2 = {
+        .config = imx_pwm_config_v2,
+        .set_enable = imx_pwm_set_enable_v2,
+};
+
+static const struct of_device_id imx_pwm_dt_ids[] = {
+        { .compatible = "fsl,imx1-pwm", .data = &imx_pwm_data_v1, },
+        { .compatible = "fsl,imx27-pwm", .data = &imx_pwm_data_v2, },
+        { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx_pwm_dt_ids);
+
 static int __devinit imx_pwm_probe(struct platform_device *pdev)
 {
+        const struct of_device_id *of_id =
+                        of_match_device(imx_pwm_dt_ids, &pdev->dev);
+        struct imx_pwm_data *data;
         struct imx_chip *imx;
         struct resource *r;
         int ret = 0;
 
+        if (!of_id)
+                return -ENODEV;
+
         imx = devm_kzalloc(&pdev->dev, sizeof(*imx), GFP_KERNEL);
         if (imx == NULL) {
                 dev_err(&pdev->dev, "failed to allocate memory\n");
                 return -ENOMEM;
         }
 
-        imx->clk = devm_clk_get(&pdev->dev, "pwm");
+        imx->clk_per = devm_clk_get(&pdev->dev, "per");
+        if (IS_ERR(imx->clk_per)) {
+                dev_err(&pdev->dev, "getting per clock failed with %ld\n",
+                                PTR_ERR(imx->clk_per));
+                return PTR_ERR(imx->clk_per);
+        }
 
-        if (IS_ERR(imx->clk))
-                return PTR_ERR(imx->clk);
+        imx->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+        if (IS_ERR(imx->clk_ipg)) {
+                dev_err(&pdev->dev, "getting ipg clock failed with %ld\n",
+                                PTR_ERR(imx->clk_ipg));
+                return PTR_ERR(imx->clk_ipg);
+        }
 
         imx->chip.ops = &imx_pwm_ops;
         imx->chip.dev = &pdev->dev;
         imx->chip.base = -1;
         imx->chip.npwm = 1;
 
-        imx->clk_enabled = 0;
-
         r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (r == NULL) {
                 dev_err(&pdev->dev, "no memory resource defined\n");
@@ -187,6 +278,10 @@ static int __devinit imx_pwm_probe(struct platform_device *pdev)
         if (imx->mmio_base == NULL)
                 return -EADDRNOTAVAIL;
 
+        data = of_id->data;
+        imx->config = data->config;
+        imx->set_enable = data->set_enable;
+
         ret = pwmchip_add(&imx->chip);
         if (ret < 0)
                 return ret;
@@ -208,23 +303,14 @@ static int __devexit imx_pwm_remove(struct platform_device *pdev)
 
 static struct platform_driver imx_pwm_driver = {
         .driver         = {
-                .name   = "mxc_pwm",
+                .name   = "imx-pwm",
+                .of_match_table = of_match_ptr(imx_pwm_dt_ids),
         },
         .probe          = imx_pwm_probe,
         .remove         = __devexit_p(imx_pwm_remove),
 };
 
-static int __init imx_pwm_init(void)
-{
-        return platform_driver_register(&imx_pwm_driver);
-}
-arch_initcall(imx_pwm_init);
-
-static void __exit imx_pwm_exit(void)
-{
-        platform_driver_unregister(&imx_pwm_driver);
-}
-module_exit(imx_pwm_exit);
+module_platform_driver(imx_pwm_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
diff --git a/drivers/pwm/pwm-jz4740.c b/drivers/pwm/pwm-jz4740.c
new file mode 100644
index 000000000000..10250fcefb98
--- /dev/null
+++ b/drivers/pwm/pwm-jz4740.c
@@ -0,0 +1,221 @@
+/*
+ *  Copyright (C) 2010, Lars-Peter Clausen <lars@metafoo.de>
+ *  JZ4740 platform PWM support
+ *
+ *  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.
+ *
+ *  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.,
+ *  675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+
+#include <asm/mach-jz4740/gpio.h>
+#include <asm/mach-jz4740/timer.h>
+
+#define NUM_PWM 8
+
+static const unsigned int jz4740_pwm_gpio_list[NUM_PWM] = {
+        JZ_GPIO_PWM0,
+        JZ_GPIO_PWM1,
+        JZ_GPIO_PWM2,
+        JZ_GPIO_PWM3,
+        JZ_GPIO_PWM4,
+        JZ_GPIO_PWM5,
+        JZ_GPIO_PWM6,
+        JZ_GPIO_PWM7,
+};
+
+struct jz4740_pwm_chip {
+        struct pwm_chip chip;
+        struct clk *clk;
+};
+
+static inline struct jz4740_pwm_chip *to_jz4740(struct pwm_chip *chip)
+{
+        return container_of(chip, struct jz4740_pwm_chip, chip);
+}
+
+static int jz4740_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+        unsigned int gpio = jz4740_pwm_gpio_list[pwm->hwpwm];
+        int ret;
+
+        /*
+         * Timers 0 and 1 are used for system tasks, so they are unavailable
+         * for use as PWMs.
+         */
+        if (pwm->hwpwm < 2)
+                return -EBUSY;
+
+        ret = gpio_request(gpio, pwm->label);
+        if (ret) {
+                dev_err(chip->dev, "Failed to request GPIO#%u for PWM: %d\n",
+                        gpio, ret);
+                return ret;
+        }
+
+        jz_gpio_set_function(gpio, JZ_GPIO_FUNC_PWM);
+
+        jz4740_timer_start(pwm->hwpwm);
+
+        return 0;
+}
+
+static void jz4740_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+        unsigned int gpio = jz4740_pwm_gpio_list[pwm->hwpwm];
+
+        jz4740_timer_set_ctrl(pwm->hwpwm, 0);
+
+        jz_gpio_set_function(gpio, JZ_GPIO_FUNC_NONE);
+        gpio_free(gpio);
+
+        jz4740_timer_stop(pwm->hwpwm);
+}
+
+static int jz4740_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+        uint32_t ctrl = jz4740_timer_get_ctrl(pwm->pwm);
+
+        ctrl |= JZ_TIMER_CTRL_PWM_ENABLE;
+        jz4740_timer_set_ctrl(pwm->hwpwm, ctrl);
+        jz4740_timer_enable(pwm->hwpwm);
+
+        return 0;
+}
+
+static void jz4740_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+        uint32_t ctrl = jz4740_timer_get_ctrl(pwm->hwpwm);
+
+        ctrl &= ~JZ_TIMER_CTRL_PWM_ENABLE;
+        jz4740_timer_disable(pwm->hwpwm);
+        jz4740_timer_set_ctrl(pwm->hwpwm, ctrl);
+}
+
+static int jz4740_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+                             int duty_ns, int period_ns)
+{
+        struct jz4740_pwm_chip *jz4740 = to_jz4740(pwm->chip);
+        unsigned long long tmp;
+        unsigned long period, duty;
+        unsigned int prescaler = 0;
+        uint16_t ctrl;
+        bool is_enabled;
+
+        tmp = (unsigned long long)clk_get_rate(jz4740->clk) * period_ns;
+        do_div(tmp, 1000000000);
+        period = tmp;
+
+        while (period > 0xffff && prescaler < 6) {
+                period >>= 2;
+                ++prescaler;
+        }
+
+        if (prescaler == 6)
+                return -EINVAL;
+
+        tmp = (unsigned long long)period * duty_ns;
+        do_div(tmp, period_ns);
+        duty = period - tmp;
+
+        if (duty >= period)
+                duty = period - 1;
+
+        is_enabled = jz4740_timer_is_enabled(pwm->hwpwm);
+        if (is_enabled)
+                jz4740_pwm_disable(chip, pwm);
+
+        jz4740_timer_set_count(pwm->hwpwm, 0);
+        jz4740_timer_set_duty(pwm->hwpwm, duty);
+        jz4740_timer_set_period(pwm->hwpwm, period);
+
+        ctrl = JZ_TIMER_CTRL_PRESCALER(prescaler) | JZ_TIMER_CTRL_SRC_EXT |
+                JZ_TIMER_CTRL_PWM_ABBRUPT_SHUTDOWN;
+
+        jz4740_timer_set_ctrl(pwm->hwpwm, ctrl);
+
+        if (is_enabled)
+                jz4740_pwm_enable(chip, pwm);
+
+        return 0;
+}
+
+static const struct pwm_ops jz4740_pwm_ops = {
+        .request = jz4740_pwm_request,
+        .free = jz4740_pwm_free,
+        .config = jz4740_pwm_config,
+        .enable = jz4740_pwm_enable,
+        .disable = jz4740_pwm_disable,
+        .owner = THIS_MODULE,
+};
+
+static int __devinit jz4740_pwm_probe(struct platform_device *pdev)
+{
+        struct jz4740_pwm_chip *jz4740;
+        int ret;
+
+        jz4740 = devm_kzalloc(&pdev->dev, sizeof(*jz4740), GFP_KERNEL);
+        if (!jz4740)
+                return -ENOMEM;
+
+        jz4740->clk = clk_get(NULL, "ext");
+        if (IS_ERR(jz4740->clk))
+                return PTR_ERR(jz4740->clk);
+
+        jz4740->chip.dev = &pdev->dev;
+        jz4740->chip.ops = &jz4740_pwm_ops;
+        jz4740->chip.npwm = NUM_PWM;
+        jz4740->chip.base = -1;
+
+        ret = pwmchip_add(&jz4740->chip);
+        if (ret < 0) {
+                clk_put(jz4740->clk);
+                return ret;
+        }
+
+        platform_set_drvdata(pdev, jz4740);
+
+        return 0;
+}
+
+static int __devexit jz4740_pwm_remove(struct platform_device *pdev)
+{
+        struct jz4740_pwm_chip *jz4740 = platform_get_drvdata(pdev);
+        int ret;
+
+        ret = pwmchip_remove(&jz4740->chip);
+        if (ret < 0)
+                return ret;
+
+        clk_put(jz4740->clk);
+
+        return 0;
+}
+
+static struct platform_driver jz4740_pwm_driver = {
+        .driver = {
+                .name = "jz4740-pwm",
+                .owner = THIS_MODULE,
+        },
+        .probe = jz4740_pwm_probe,
+        .remove = __devexit_p(jz4740_pwm_remove),
+};
+module_platform_driver(jz4740_pwm_driver);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Ingenic JZ4740 PWM driver");
+MODULE_ALIAS("platform:jz4740-pwm");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pwm/pwm-puv3.c b/drivers/pwm/pwm-puv3.c
new file mode 100644
index 000000000000..2a93f37c46ad
--- /dev/null
+++ b/drivers/pwm/pwm-puv3.c
@@ -0,0 +1,161 @@
+/*
+ * linux/arch/unicore32/kernel/pwm.c
+ *
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *      Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *      Copyright (C) 2001-2010 Guan Xuetao
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+
+#include <asm/div64.h>
+#include <mach/hardware.h>
+
+struct puv3_pwm_chip {
+        struct pwm_chip chip;
+        void __iomem *base;
+        struct clk *clk;
+        bool enabled;
+};
+
+static inline struct puv3_pwm_chip *to_puv3(struct pwm_chip *chip)
+{
+        return container_of(chip, struct puv3_pwm_chip, chip);
+}
+
+/*
+ * period_ns = 10^9 * (PRESCALE + 1) * (PV + 1) / PWM_CLK_RATE
+ * duty_ns   = 10^9 * (PRESCALE + 1) * DC / PWM_CLK_RATE
+ */
+static int puv3_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+                           int duty_ns, int period_ns)
+{
+        unsigned long period_cycles, prescale, pv, dc;
+        struct puv3_pwm_chip *puv3 = to_puv3(chip);
+        unsigned long long c;
+
+        c = clk_get_rate(puv3->clk);
+        c = c * period_ns;
+        do_div(c, 1000000000);
+        period_cycles = c;
+
+        if (period_cycles < 1)
+                period_cycles = 1;
+
+        prescale = (period_cycles - 1) / 1024;
+        pv = period_cycles / (prescale + 1) - 1;
+
+        if (prescale > 63)
+                return -EINVAL;
+
+        if (duty_ns == period_ns)
+                dc = OST_PWMDCCR_FDCYCLE;
+        else
+                dc = (pv + 1) * duty_ns / period_ns;
+
+        /*
+         * NOTE: the clock to PWM has to be enabled first
+         * before writing to the registers
+         */
+        clk_prepare_enable(puv3->clk);
+
+        writel(prescale, puv3->base + OST_PWM_PWCR);
+        writel(pv - dc, puv3->base + OST_PWM_DCCR);
+        writel(pv, puv3->base + OST_PWM_PCR);
+
+        clk_disable_unprepare(puv3->clk);
+
+        return 0;
+}
+
+static int puv3_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+        struct puv3_pwm_chip *puv3 = to_puv3(chip);
+
+        return clk_prepare_enable(puv3->clk);
+}
+
+static void puv3_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+        struct puv3_pwm_chip *puv3 = to_puv3(chip);
+
+        clk_disable_unprepare(puv3->clk);
+}
+
+static const struct pwm_ops puv3_pwm_ops = {
+        .config = puv3_pwm_config,
+        .enable = puv3_pwm_enable,
+        .disable = puv3_pwm_disable,
+        .owner = THIS_MODULE,
+};
+
+static int __devinit pwm_probe(struct platform_device *pdev)
+{
+        struct puv3_pwm_chip *puv3;
+        struct resource *r;
+        int ret;
+
+        puv3 = devm_kzalloc(&pdev->dev, sizeof(*puv3), GFP_KERNEL);
+        if (puv3 == NULL) {
+                dev_err(&pdev->dev, "failed to allocate memory\n");
+                return -ENOMEM;
+        }
+
+        puv3->clk = devm_clk_get(&pdev->dev, "OST_CLK");
+        if (IS_ERR(puv3->clk))
+                return PTR_ERR(puv3->clk);
+
+        r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (r == NULL) {
+                dev_err(&pdev->dev, "no memory resource defined\n");
+                return -ENODEV;
+        }
+
+        puv3->base = devm_request_and_ioremap(&pdev->dev, r);
+        if (puv3->base == NULL)
+                return -EADDRNOTAVAIL;
+
+        puv3->chip.dev = &pdev->dev;
+        puv3->chip.ops = &puv3_pwm_ops;
+        puv3->chip.base = -1;
+        puv3->chip.npwm = 1;
+
+        ret = pwmchip_add(&puv3->chip);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+                return ret;
+        }
+
+        platform_set_drvdata(pdev, puv3);
+        return 0;
+}
+
+static int __devexit pwm_remove(struct platform_device *pdev)
+{
+        struct puv3_pwm_chip *puv3 = platform_get_drvdata(pdev);
+
+        return pwmchip_remove(&puv3->chip);
+}
+
+static struct platform_driver puv3_pwm_driver = {
+        .driver = {
+                .name = "PKUnity-v3-PWM",
+        },
+        .probe = pwm_probe,
+        .remove = __devexit_p(pwm_remove),
+};
+module_platform_driver(puv3_pwm_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-pxa.c b/drivers/pwm/pwm-pxa.c
index bd5867a1c700..260c3a88564d 100644
--- a/drivers/pwm/pwm-pxa.c
+++ b/drivers/pwm/pwm-pxa.c
@@ -70,9 +70,6 @@ static int pxa_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         unsigned long offset;
         int rc;
 
-        if (period_ns == 0 || duty_ns > period_ns)
-                return -EINVAL;
-
         offset = pwm->hwpwm ? 0x10 : 0;
 
         c = clk_get_rate(pc->clk);
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
index e5187c0ade9f..023a3bee76e7 100644
--- a/drivers/pwm/pwm-samsung.c
+++ b/drivers/pwm/pwm-samsung.c
@@ -126,9 +126,6 @@ static int s3c_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         if (period_ns > NS_IN_HZ || duty_ns > NS_IN_HZ)
                 return -ERANGE;
 
-        if (duty_ns > period_ns)
-                return -EINVAL;
-
         if (period_ns == s3c->period_ns &&
             duty_ns == s3c->duty_ns)
                 return 0;
diff --git a/drivers/pwm/pwm-tiecap.c b/drivers/pwm/pwm-tiecap.c
index 4b6688909fee..d6d4cf05565e 100644
--- a/drivers/pwm/pwm-tiecap.c
+++ b/drivers/pwm/pwm-tiecap.c
@@ -32,6 +32,7 @@
 #define CAP3                    0x10
 #define CAP4                    0x14
 #define ECCTL2                  0x2A
+#define ECCTL2_APWM_POL_LOW     BIT(10)
 #define ECCTL2_APWM_MODE        BIT(9)
 #define ECCTL2_SYNC_SEL_DISA    (BIT(7) | BIT(6))
 #define ECCTL2_TSCTR_FREERUN    BIT(4)
@@ -59,7 +60,7 @@ static int ecap_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         unsigned long period_cycles, duty_cycles;
         unsigned int reg_val;
 
-        if (period_ns < 0 || duty_ns < 0 || period_ns > NSEC_PER_SEC)
+        if (period_ns > NSEC_PER_SEC)
                 return -ERANGE;
 
         c = pc->clk_rate;
@@ -111,6 +112,26 @@ static int ecap_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         return 0;
 }
 
+static int ecap_pwm_set_polarity(struct pwm_chip *chip, struct pwm_device *pwm,
+                enum pwm_polarity polarity)
+{
+        struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
+        unsigned short reg_val;
+
+        pm_runtime_get_sync(pc->chip.dev);
+        reg_val = readw(pc->mmio_base + ECCTL2);
+        if (polarity == PWM_POLARITY_INVERSED)
+                /* Duty cycle defines LOW period of PWM */
+                reg_val |= ECCTL2_APWM_POL_LOW;
+        else
+                /* Duty cycle defines HIGH period of PWM */
+                reg_val &= ~ECCTL2_APWM_POL_LOW;
+
+        writew(reg_val, pc->mmio_base + ECCTL2);
+        pm_runtime_put_sync(pc->chip.dev);
+        return 0;
+}
+
 static int ecap_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
         struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
@@ -157,6 +178,7 @@ static void ecap_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
 static const struct pwm_ops ecap_pwm_ops = {
         .free           = ecap_pwm_free,
         .config         = ecap_pwm_config,
+        .set_polarity   = ecap_pwm_set_polarity,
         .enable         = ecap_pwm_enable,
         .disable        = ecap_pwm_disable,
         .owner          = THIS_MODULE,
diff --git a/drivers/pwm/pwm-tiehrpwm.c b/drivers/pwm/pwm-tiehrpwm.c
index b1996bcd5b78..d3c1dff0a0dc 100644
--- a/drivers/pwm/pwm-tiehrpwm.c
+++ b/drivers/pwm/pwm-tiehrpwm.c
@@ -81,6 +81,15 @@
 #define AQCTL_ZRO_FRCHIGH       BIT(1)
 #define AQCTL_ZRO_FRCTOGGLE     (BIT(1) | BIT(0))
 
+#define AQCTL_CHANA_POLNORMAL   (AQCTL_CAU_FRCLOW | AQCTL_PRD_FRCHIGH | \
+                                AQCTL_ZRO_FRCHIGH)
+#define AQCTL_CHANA_POLINVERSED (AQCTL_CAU_FRCHIGH | AQCTL_PRD_FRCLOW | \
+                                AQCTL_ZRO_FRCLOW)
+#define AQCTL_CHANB_POLNORMAL   (AQCTL_CBU_FRCLOW | AQCTL_PRD_FRCHIGH | \
+                                AQCTL_ZRO_FRCHIGH)
+#define AQCTL_CHANB_POLINVERSED (AQCTL_CBU_FRCHIGH | AQCTL_PRD_FRCLOW | \
+                                AQCTL_ZRO_FRCLOW)
+
 #define AQSFRC_RLDCSF_MASK      (BIT(7) | BIT(6))
 #define AQSFRC_RLDCSF_ZRO       0
 #define AQSFRC_RLDCSF_PRD       BIT(6)
@@ -105,6 +114,7 @@ struct ehrpwm_pwm_chip {
         unsigned int    clk_rate;
         void __iomem    *mmio_base;
         unsigned long period_cycles[NUM_PWM_CHANNEL];
+        enum pwm_polarity polarity[NUM_PWM_CHANNEL];
 };
 
 static inline struct ehrpwm_pwm_chip *to_ehrpwm_pwm_chip(struct pwm_chip *chip)
@@ -165,39 +175,37 @@ static int set_prescale_div(unsigned long rqst_prescaler,
         return 1;
 }
 
-static void configure_chans(struct ehrpwm_pwm_chip *pc, int chan,
-                unsigned long duty_cycles)
+static void configure_polarity(struct ehrpwm_pwm_chip *pc, int chan)
 {
-        int cmp_reg, aqctl_reg;
+        int aqctl_reg;
         unsigned short aqctl_val, aqctl_mask;
 
         /*
-         * Channels can be configured from action qualifier module.
-         * Channel 0 configured with compare A register and for
-         * up-counter mode.
-         * Channel 1 configured with compare B register and for
-         * up-counter mode.
+         * Configure PWM output to HIGH/LOW level on counter
+         * reaches compare register value and LOW/HIGH level
+         * on counter value reaches period register value and
+         * zero value on counter
          */
         if (chan == 1) {
                 aqctl_reg = AQCTLB;
-                cmp_reg = CMPB;
-                /* Configure PWM Low from compare B value */
-                aqctl_val = AQCTL_CBU_FRCLOW;
                 aqctl_mask = AQCTL_CBU_MASK;
+
+                if (pc->polarity[chan] == PWM_POLARITY_INVERSED)
+                        aqctl_val = AQCTL_CHANB_POLINVERSED;
+                else
+                        aqctl_val = AQCTL_CHANB_POLNORMAL;
         } else {
-                cmp_reg = CMPA;
                 aqctl_reg = AQCTLA;
-                /* Configure PWM Low from compare A value*/
-                aqctl_val = AQCTL_CAU_FRCLOW;
                 aqctl_mask = AQCTL_CAU_MASK;
+
+                if (pc->polarity[chan] == PWM_POLARITY_INVERSED)
+                        aqctl_val = AQCTL_CHANA_POLINVERSED;
+                else
+                        aqctl_val = AQCTL_CHANA_POLNORMAL;
         }
 
-        /* Configure PWM High from period value and zero value */
-        aqctl_val |= AQCTL_PRD_FRCHIGH | AQCTL_ZRO_FRCHIGH;
         aqctl_mask |= AQCTL_PRD_MASK | AQCTL_ZRO_MASK;
-        ehrpwm_modify(pc->mmio_base,  aqctl_reg, aqctl_mask, aqctl_val);
-
-        ehrpwm_write(pc->mmio_base,  cmp_reg, duty_cycles);
+        ehrpwm_modify(pc->mmio_base, aqctl_reg, aqctl_mask, aqctl_val);
 }
 
 /*
@@ -211,9 +219,9 @@ static int ehrpwm_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         unsigned long long c;
         unsigned long period_cycles, duty_cycles;
         unsigned short ps_divval, tb_divval;
-        int i;
+        int i, cmp_reg;
 
-        if (period_ns < 0 || duty_ns < 0 || period_ns > NSEC_PER_SEC)
+        if (period_ns > NSEC_PER_SEC)
                 return -ERANGE;
 
         c = pc->clk_rate;
@@ -278,12 +286,29 @@ static int ehrpwm_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
         ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_CTRMODE_MASK,
                         TBCTL_CTRMODE_UP);
 
-        /* Configure the channel for duty cycle */
-        configure_chans(pc, pwm->hwpwm, duty_cycles);
+        if (pwm->hwpwm == 1)
+                /* Channel 1 configured with compare B register */
+                cmp_reg = CMPB;
+        else
+                /* Channel 0 configured with compare A register */
+                cmp_reg = CMPA;
+
+        ehrpwm_write(pc->mmio_base, cmp_reg, duty_cycles);
+
         pm_runtime_put_sync(chip->dev);
         return 0;
 }
 
+static int ehrpwm_pwm_set_polarity(struct pwm_chip *chip,
+                struct pwm_device *pwm, enum pwm_polarity polarity)
+{
+        struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
+
+        /* Configuration of polarity in hardware delayed, do at enable */
+        pc->polarity[pwm->hwpwm] = polarity;
+        return 0;
+}
+
 static int ehrpwm_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
         struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
@@ -307,6 +332,9 @@ static int ehrpwm_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 
         ehrpwm_modify(pc->mmio_base, AQCSFRC, aqcsfrc_mask, aqcsfrc_val);
 
+        /* Channels polarity can be configured from action qualifier module */
+        configure_polarity(pc, pwm->hwpwm);
+
         /* Enable time counter for free_run */
         ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_RUN_MASK, TBCTL_FREE_RUN);
         return 0;
@@ -358,6 +386,7 @@ static void ehrpwm_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
 static const struct pwm_ops ehrpwm_pwm_ops = {
         .free           = ehrpwm_pwm_free,
         .config         = ehrpwm_pwm_config,
+        .set_polarity   = ehrpwm_pwm_set_polarity,
         .enable         = ehrpwm_pwm_enable,
         .disable        = ehrpwm_pwm_disable,
         .owner          = THIS_MODULE,
diff --git a/drivers/rapidio/rio-scan.c b/drivers/rapidio/rio-scan.c
index 48e9041dd1e2..07da58bb495c 100644
--- a/drivers/rapidio/rio-scan.c
+++ b/drivers/rapidio/rio-scan.c
@@ -55,9 +55,9 @@ static int rio_mport_phys_table[] = {
 };
 
 
-/*
+/**
  * rio_destid_alloc - Allocate next available destID for given network
- * net: RIO network
+ * @net: RIO network
  *
  * Returns next available device destination ID for the specified RIO network.
  * Marks allocated ID as one in use.
@@ -69,14 +69,9 @@ static u16 rio_destid_alloc(struct rio_net *net)
         struct rio_id_table *idtab = &net->destid_table;
 
         spin_lock(&idtab->lock);
-        destid = find_next_zero_bit(idtab->table, idtab->max, idtab->next);
-        if (destid >= idtab->max)
-                destid = find_first_zero_bit(idtab->table, idtab->max);
+        destid = find_first_zero_bit(idtab->table, idtab->max);
 
         if (destid < idtab->max) {
-                idtab->next = destid + 1;
-                if (idtab->next >= idtab->max)
-                        idtab->next = 0;
                 set_bit(destid, idtab->table);
                 destid += idtab->start;
         } else
@@ -86,10 +81,10 @@ static u16 rio_destid_alloc(struct rio_net *net)
         return (u16)destid;
 }
 
-/*
+/**
  * rio_destid_reserve - Reserve the specivied destID
- * net: RIO network
- * destid: destID to reserve
+ * @net: RIO network
+ * @destid: destID to reserve
  *
  * Tries to reserve the specified destID.
  * Returns 0 if successfull.
@@ -106,10 +101,10 @@ static int rio_destid_reserve(struct rio_net *net, u16 destid)
         return oldbit;
 }
 
-/*
+/**
  * rio_destid_free - free a previously allocated destID
- * net: RIO network
- * destid: destID to free
+ * @net: RIO network
+ * @destid: destID to free
  *
  * Makes the specified destID available for use.
  */
@@ -123,9 +118,9 @@ static void rio_destid_free(struct rio_net *net, u16 destid)
         spin_unlock(&idtab->lock);
 }
 
-/*
+/**
  * rio_destid_first - return first destID in use
- * net: RIO network
+ * @net: RIO network
  */
 static u16 rio_destid_first(struct rio_net *net)
 {
@@ -142,10 +137,10 @@ static u16 rio_destid_first(struct rio_net *net)
         return (u16)destid;
 }
 
-/*
+/**
  * rio_destid_next - return next destID in use
- * net: RIO network
- * from: destination ID from which search shall continue
+ * @net: RIO network
+ * @from: destination ID from which search shall continue
  */
 static u16 rio_destid_next(struct rio_net *net, u16 from)
 {
@@ -1163,8 +1158,8 @@ static struct rio_net __devinit *rio_alloc_net(struct rio_mport *port,
 
         net = kzalloc(sizeof(struct rio_net), GFP_KERNEL);
         if (net && do_enum) {
-                net->destid_table.table = kzalloc(
-                        BITS_TO_LONGS(RIO_MAX_ROUTE_ENTRIES(port->sys_size)) *
+                net->destid_table.table = kcalloc(
+                        BITS_TO_LONGS(RIO_MAX_ROUTE_ENTRIES(port->sys_size)),
                         sizeof(long),
                         GFP_KERNEL);
 
@@ -1174,7 +1169,6 @@ static struct rio_net __devinit *rio_alloc_net(struct rio_mport *port,
                         net = NULL;
                 } else {
                         net->destid_table.start = start;
-                        net->destid_table.next = 0;
                         net->destid_table.max =
                                         RIO_MAX_ROUTE_ENTRIES(port->sys_size);
                         spin_lock_init(&net->destid_table.lock);
@@ -1391,7 +1385,7 @@ int __devinit rio_disc_mport(struct rio_mport *mport)
                 while (time_before(jiffies, to_end)) {
                         if (rio_enum_complete(mport))
                                 goto enum_done;
-                        schedule_timeout_uninterruptible(msecs_to_jiffies(10));
+                        msleep(10);
                 }
 
                 pr_debug("RIO: discovery timeout on mport %d %s\n",
diff --git a/drivers/rapidio/rio.c b/drivers/rapidio/rio.c
index d4bd69013c50..c17ae22567e0 100644
--- a/drivers/rapidio/rio.c
+++ b/drivers/rapidio/rio.c
@@ -1275,49 +1275,68 @@ static void __devinit disc_work_handler(struct work_struct *_work)
         pr_debug("RIO: discovery work for mport %d %s\n",
                  work->mport->id, work->mport->name);
         rio_disc_mport(work->mport);
-
-        kfree(work);
 }
 
 int __devinit rio_init_mports(void)
 {
         struct rio_mport *port;
         struct rio_disc_work *work;
-        int no_disc = 0;
+        int n = 0;
+
+        if (!next_portid)
+                return -ENODEV;
 
+        /*
+         * First, run enumerations and check if we need to perform discovery
+         * on any of the registered mports.
+         */
         list_for_each_entry(port, &rio_mports, node) {
                 if (port->host_deviceid >= 0)
                         rio_enum_mport(port);
-                else if (!no_disc) {
-                        if (!rio_wq) {
-                                rio_wq = alloc_workqueue("riodisc", 0, 0);
-                                if (!rio_wq) {
-                                        pr_err("RIO: unable allocate rio_wq\n");
-                                        no_disc = 1;
-                                        continue;
-                                }
-                        }
-
-                        work = kzalloc(sizeof *work, GFP_KERNEL);
-                        if (!work) {
-                                pr_err("RIO: no memory for work struct\n");
-                                no_disc = 1;
-                                continue;
-                        }
-
-                        work->mport = port;
-                        INIT_WORK(&work->work, disc_work_handler);
-                        queue_work(rio_wq, &work->work);
-                }
+                else
+                        n++;
+        }
+
+        if (!n)
+                goto no_disc;
+
+        /*
+         * If we have mports that require discovery schedule a discovery work
+         * for each of them. If the code below fails to allocate needed
+         * resources, exit without error to keep results of enumeration
+         * process (if any).
+         * TODO: Implement restart of dicovery process for all or
+         * individual discovering mports.
+         */
+        rio_wq = alloc_workqueue("riodisc", 0, 0);
+        if (!rio_wq) {
+                pr_err("RIO: unable allocate rio_wq\n");
+                goto no_disc;
         }
 
-        if (rio_wq) {
-                pr_debug("RIO: flush discovery workqueue\n");
-                flush_workqueue(rio_wq);
-                pr_debug("RIO: flush discovery workqueue finished\n");
+        work = kcalloc(n, sizeof *work, GFP_KERNEL);
+        if (!work) {
+                pr_err("RIO: no memory for work struct\n");
                 destroy_workqueue(rio_wq);
+                goto no_disc;
         }
 
+        n = 0;
+        list_for_each_entry(port, &rio_mports, node) {
+                if (port->host_deviceid < 0) {
+                        work[n].mport = port;
+                        INIT_WORK(&work[n].work, disc_work_handler);
+                        queue_work(rio_wq, &work[n].work);
+                        n++;
+                }
+        }
+
+        flush_workqueue(rio_wq);
+        pr_debug("RIO: destroy discovery workqueue\n");
+        destroy_workqueue(rio_wq);
+        kfree(work);
+
+no_disc:
         rio_init();
 
         return 0;
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index e069f176a82d..19c03ab2bdcb 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -59,6 +59,7 @@ comment "RTC interfaces"
 config RTC_INTF_SYSFS
         boolean "/sys/class/rtc/rtcN (sysfs)"
         depends on SYSFS
+        default RTC_CLASS
         help
           Say yes here if you want to use your RTCs using sysfs interfaces,
           /sys/class/rtc/rtc0 through /sys/.../rtcN.
@@ -68,6 +69,7 @@ config RTC_INTF_SYSFS
 config RTC_INTF_PROC
         boolean "/proc/driver/rtc (procfs for rtcN)"
         depends on PROC_FS
+        default RTC_CLASS
         help
           Say yes here if you want to use your system clock RTC through
           the proc interface, /proc/driver/rtc.
@@ -79,6 +81,7 @@ config RTC_INTF_PROC
 
 config RTC_INTF_DEV
         boolean "/dev/rtcN (character devices)"
+        default RTC_CLASS
         help
           Say yes here if you want to use your RTCs using the /dev
           interfaces, which "udev" sets up as /dev/rtc0 through
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index 891cd6c61d0a..4eed51044c5d 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -392,6 +392,8 @@ static int dryice_rtc_probe(struct platform_device *pdev)
         if (imxdi->ioaddr == NULL)
                 return -ENOMEM;
 
+        spin_lock_init(&imxdi->irq_lock);
+
         imxdi->irq = platform_get_irq(pdev, 0);
         if (imxdi->irq < 0)
                 return imxdi->irq;
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index a5a55da2a1ac..b6ad0de07930 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -69,23 +69,9 @@ static ssize_t dcssblk_add_store(struct device * dev, struct device_attribute *a
                                   size_t count);
 static ssize_t dcssblk_remove_store(struct device * dev, struct device_attribute *attr, const char * buf,
                                   size_t count);
-static ssize_t dcssblk_save_store(struct device * dev, struct device_attribute *attr, const char * buf,
-                                  size_t count);
-static ssize_t dcssblk_save_show(struct device *dev, struct device_attribute *attr, char *buf);
-static ssize_t dcssblk_shared_store(struct device * dev, struct device_attribute *attr, const char * buf,
-                                  size_t count);
-static ssize_t dcssblk_shared_show(struct device *dev, struct device_attribute *attr, char *buf);
-static ssize_t dcssblk_seglist_show(struct device *dev,
-                                struct device_attribute *attr,
-                                char *buf);
 
 static DEVICE_ATTR(add, S_IWUSR, NULL, dcssblk_add_store);
 static DEVICE_ATTR(remove, S_IWUSR, NULL, dcssblk_remove_store);
-static DEVICE_ATTR(save, S_IWUSR | S_IRUSR, dcssblk_save_show,
-                   dcssblk_save_store);
-static DEVICE_ATTR(shared, S_IWUSR | S_IRUSR, dcssblk_shared_show,
-                   dcssblk_shared_store);
-static DEVICE_ATTR(seglist, S_IRUSR, dcssblk_seglist_show, NULL);
 
 static struct device *dcssblk_root_dev;
 
@@ -416,6 +402,8 @@ out:
         up_write(&dcssblk_devices_sem);
         return rc;
 }
+static DEVICE_ATTR(shared, S_IWUSR | S_IRUSR, dcssblk_shared_show,
+                   dcssblk_shared_store);
 
 /*
  * device attribute for save operation on current copy
@@ -476,6 +464,8 @@ dcssblk_save_store(struct device *dev, struct device_attribute *attr, const char
         up_write(&dcssblk_devices_sem);
         return count;
 }
+static DEVICE_ATTR(save, S_IWUSR | S_IRUSR, dcssblk_save_show,
+                   dcssblk_save_store);
 
 /*
  * device attribute for showing all segments in a device
@@ -502,6 +492,21 @@ dcssblk_seglist_show(struct device *dev, struct device_attribute *attr,
         up_read(&dcssblk_devices_sem);
         return i;
 }
+static DEVICE_ATTR(seglist, S_IRUSR, dcssblk_seglist_show, NULL);
+
+static struct attribute *dcssblk_dev_attrs[] = {
+        &dev_attr_shared.attr,
+        &dev_attr_save.attr,
+        &dev_attr_seglist.attr,
+        NULL,
+};
+static struct attribute_group dcssblk_dev_attr_group = {
+        .attrs = dcssblk_dev_attrs,
+};
+static const struct attribute_group *dcssblk_dev_attr_groups[] = {
+        &dcssblk_dev_attr_group,
+        NULL,
+};
 
 /*
  * device attribute for adding devices
@@ -590,6 +595,7 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
 
         dev_set_name(&dev_info->dev, dev_info->segment_name);
         dev_info->dev.release = dcssblk_release_segment;
+        dev_info->dev.groups = dcssblk_dev_attr_groups;
         INIT_LIST_HEAD(&dev_info->lh);
         dev_info->gd = alloc_disk(DCSSBLK_MINORS_PER_DISK);
         if (dev_info->gd == NULL) {
@@ -637,21 +643,10 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
          * register the device
          */
         rc = device_register(&dev_info->dev);
-        if (rc) {
-                module_put(THIS_MODULE);
-                goto dev_list_del;
-        }
-        get_device(&dev_info->dev);
-        rc = device_create_file(&dev_info->dev, &dev_attr_shared);
-        if (rc)
-                goto unregister_dev;
-        rc = device_create_file(&dev_info->dev, &dev_attr_save);
-        if (rc)
-                goto unregister_dev;
-        rc = device_create_file(&dev_info->dev, &dev_attr_seglist);
         if (rc)
-                goto unregister_dev;
+                goto put_dev;
 
+        get_device(&dev_info->dev);
         add_disk(dev_info->gd);
 
         switch (dev_info->segment_type) {
@@ -668,12 +663,11 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
         rc = count;
         goto out;
 
-unregister_dev:
+put_dev:
         list_del(&dev_info->lh);
         blk_cleanup_queue(dev_info->dcssblk_queue);
         dev_info->gd->queue = NULL;
         put_disk(dev_info->gd);
-        device_unregister(&dev_info->dev);
         list_for_each_entry(seg_info, &dev_info->seg_list, lh) {
                 segment_unload(seg_info->segment_name);
         }
diff --git a/drivers/s390/cio/css.c b/drivers/s390/cio/css.c
index b4d572f65f07..fd00afd8b850 100644
--- a/drivers/s390/cio/css.c
+++ b/drivers/s390/cio/css.c
@@ -377,7 +377,11 @@ static int css_evaluate_new_subchannel(struct subchannel_id schid, int slow)
                 /* Will be done on the slow path. */
                 return -EAGAIN;
         }
-        if (stsch_err(schid, &schib) || !css_sch_is_valid(&schib)) {
+        if (stsch_err(schid, &schib)) {
+                /* Subchannel is not provided. */
+                return -ENXIO;
+        }
+        if (!css_sch_is_valid(&schib)) {
                 /* Unusable - ignore. */
                 return 0;
         }
@@ -536,6 +540,7 @@ static int slow_eval_unknown_fn(struct subchannel_id schid, void *data)
                 case -ENOMEM:
                 case -EIO:
                         /* These should abort looping */
+                        idset_sch_del_subseq(slow_subchannel_set, schid);
                         break;
                 default:
                         rc = 0;
diff --git a/drivers/s390/cio/idset.c b/drivers/s390/cio/idset.c
index e6d5f8c49524..199bc6791177 100644
--- a/drivers/s390/cio/idset.c
+++ b/drivers/s390/cio/idset.c
@@ -1,9 +1,10 @@
 /*
- *    Copyright IBM Corp. 2007
+ *    Copyright IBM Corp. 2007, 2012
  *    Author(s): Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
  */
 
 #include <linux/vmalloc.h>
+#include <linux/bitmap.h>
 #include <linux/bitops.h>
 #include "idset.h"
 #include "css.h"
@@ -89,6 +90,14 @@ void idset_sch_del(struct idset *set, struct subchannel_id schid)
         idset_del(set, schid.ssid, schid.sch_no);
 }
 
+/* Clear ids starting from @schid up to end of subchannel set. */
+void idset_sch_del_subseq(struct idset *set, struct subchannel_id schid)
+{
+        int pos = schid.ssid * set->num_id + schid.sch_no;
+
+        bitmap_clear(set->bitmap, pos, set->num_id - schid.sch_no);
+}
+
 int idset_sch_contains(struct idset *set, struct subchannel_id schid)
 {
         return idset_contains(set, schid.ssid, schid.sch_no);
@@ -111,20 +120,13 @@ int idset_sch_get_first(struct idset *set, struct subchannel_id *schid)
 
 int idset_is_empty(struct idset *set)
 {
-        int bitnum;
-
-        bitnum = find_first_bit(set->bitmap, set->num_ssid * set->num_id);
-        if (bitnum >= set->num_ssid * set->num_id)
-                return 1;
-        return 0;
+        return bitmap_empty(set->bitmap, set->num_ssid * set->num_id);
 }
 
 void idset_add_set(struct idset *to, struct idset *from)
 {
-        unsigned long i, len;
+        int len = min(__BITOPS_WORDS(to->num_ssid * to->num_id),
+                      __BITOPS_WORDS(from->num_ssid * from->num_id));
 
-        len = min(__BITOPS_WORDS(to->num_ssid * to->num_id),
-                  __BITOPS_WORDS(from->num_ssid * from->num_id));
-        for (i = 0; i < len ; i++)
-                to->bitmap[i] |= from->bitmap[i];
+        bitmap_or(to->bitmap, to->bitmap, from->bitmap, len);
 }
diff --git a/drivers/s390/cio/idset.h b/drivers/s390/cio/idset.h
index 3d943f03591e..06d3bc01bb09 100644
--- a/drivers/s390/cio/idset.h
+++ b/drivers/s390/cio/idset.h
@@ -1,5 +1,5 @@
 /*
- *    Copyright IBM Corp. 2007
+ *    Copyright IBM Corp. 2007, 2012
  *    Author(s): Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
  */
 
@@ -17,6 +17,7 @@ void idset_fill(struct idset *set);
 struct idset *idset_sch_new(void);
 void idset_sch_add(struct idset *set, struct subchannel_id id);
 void idset_sch_del(struct idset *set, struct subchannel_id id);
+void idset_sch_del_subseq(struct idset *set, struct subchannel_id schid);
 int idset_sch_contains(struct idset *set, struct subchannel_id id);
 int idset_sch_get_first(struct idset *set, struct subchannel_id *id);
 int idset_is_empty(struct idset *set);
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.c b/drivers/s390/crypto/zcrypt_pcixcc.c
index c7275e303a0d..899ffa19f5ec 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.c
+++ b/drivers/s390/crypto/zcrypt_pcixcc.c
@@ -39,7 +39,6 @@
 #include "zcrypt_msgtype6.h"
 #include "zcrypt_pcixcc.h"
 #include "zcrypt_cca_key.h"
-#include "zcrypt_msgtype6.h"
 
 #define PCIXCC_MIN_MOD_SIZE      16     /*  128 bits    */
 #define PCIXCC_MIN_MOD_SIZE_OLD  64     /*  512 bits    */
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 2db409330c21..e67e0258aec5 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -1141,11 +1141,12 @@ static int qeth_l2_recover(void *ptr)
                 dev_info(&card->gdev->dev,
                         "Device successfully recovered!\n");
         else {
-                rtnl_lock();
-                dev_close(card->dev);
-                rtnl_unlock();
-                dev_warn(&card->gdev->dev, "The qeth device driver "
-                        "failed to recover an error on the device\n");
+                if (rtnl_trylock()) {
+                        dev_close(card->dev);
+                        rtnl_unlock();
+                        dev_warn(&card->gdev->dev, "The qeth device driver "
+                                "failed to recover an error on the device\n");
+                }
         }
         qeth_clear_thread_start_bit(card, QETH_RECOVER_THREAD);
         qeth_clear_thread_running_bit(card, QETH_RECOVER_THREAD);
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 4cd310cb5bdf..5ba390658498 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -3510,11 +3510,12 @@ static int qeth_l3_recover(void *ptr)
                 dev_info(&card->gdev->dev,
                         "Device successfully recovered!\n");
         else {
-                rtnl_lock();
-                dev_close(card->dev);
-                rtnl_unlock();
-                dev_warn(&card->gdev->dev, "The qeth device driver "
-                        "failed to recover an error on the device\n");
+                if (rtnl_trylock()) {
+                        dev_close(card->dev);
+                        rtnl_unlock();
+                        dev_warn(&card->gdev->dev, "The qeth device driver "
+                                "failed to recover an error on the device\n");
+                }
         }
         qeth_clear_thread_start_bit(card, QETH_RECOVER_THREAD);
         qeth_clear_thread_running_bit(card, QETH_RECOVER_THREAD);
diff --git a/drivers/s390/net/smsgiucv.c b/drivers/s390/net/smsgiucv.c
index 207b7d742443..d8f990b6b332 100644
--- a/drivers/s390/net/smsgiucv.c
+++ b/drivers/s390/net/smsgiucv.c
@@ -157,7 +157,7 @@ static int smsg_pm_restore_thaw(struct device *dev)
 #ifdef CONFIG_PM_DEBUG
         printk(KERN_WARNING "smsg_pm_restore_thaw\n");
 #endif
-        if (smsg_path && iucv_path_connected) {
+        if (smsg_path && !iucv_path_connected) {
                 memset(smsg_path, 0, sizeof(*smsg_path));
                 smsg_path->msglim = 255;
                 smsg_path->flags = 0;
diff --git a/drivers/scsi/arm/fas216.c b/drivers/scsi/arm/fas216.c
index 6206a666a8ec..737554c37d9e 100644
--- a/drivers/scsi/arm/fas216.c
+++ b/drivers/scsi/arm/fas216.c
@@ -179,6 +179,7 @@ static void print_SCp(struct scsi_pointer *SCp, const char *prefix, const char *
                 SCp->buffers_residual, suffix);
 }
 
+#ifdef CHECK_STRUCTURE
 static void fas216_dumpinfo(FAS216_Info *info)
 {
         static int used = 0;
@@ -223,7 +224,6 @@ static void fas216_dumpinfo(FAS216_Info *info)
                 info->internal_done, info->magic_end);
 }
 
-#ifdef CHECK_STRUCTURE
 static void __fas216_checkmagic(FAS216_Info *info, const char *func)
 {
         int corruption = 0;
diff --git a/drivers/scsi/arm/oak.c b/drivers/scsi/arm/oak.c
index d25f944b59c2..fc6a5aabf66e 100644
--- a/drivers/scsi/arm/oak.c
+++ b/drivers/scsi/arm/oak.c
@@ -21,6 +21,7 @@
 /*#define PSEUDO_DMA*/
 
 #define OAKSCSI_PUBLIC_RELEASE 1
+#define DONT_USE_INTR
 
 #define priv(host)                      ((struct NCR5380_hostdata *)(host)->hostdata)
 #define NCR5380_local_declare()         void __iomem *_base
diff --git a/drivers/scsi/bfa/bfa_core.c b/drivers/scsi/bfa/bfa_core.c
index b7c326f7a6d0..342d7d9c0997 100644
--- a/drivers/scsi/bfa/bfa_core.c
+++ b/drivers/scsi/bfa/bfa_core.c
@@ -165,6 +165,16 @@ bfa_com_phy_attach(struct bfa_s *bfa, bfa_boolean_t mincfg)
         bfa_phy_memclaim(phy, phy_dma->kva_curp, phy_dma->dma_curp, mincfg);
 }
 
+static void
+bfa_com_fru_attach(struct bfa_s *bfa, bfa_boolean_t mincfg)
+{
+        struct bfa_fru_s        *fru = BFA_FRU(bfa);
+        struct bfa_mem_dma_s    *fru_dma = BFA_MEM_FRU_DMA(bfa);
+
+        bfa_fru_attach(fru, &bfa->ioc, bfa, bfa->trcmod, mincfg);
+        bfa_fru_memclaim(fru, fru_dma->kva_curp, fru_dma->dma_curp, mincfg);
+}
+
 /*
  * BFA IOC FC related definitions
  */
@@ -274,6 +284,15 @@ bfa_iocfc_sm_initing(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
         case IOCFC_E_IOC_ENABLED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_dconf_read);
                 break;
+
+        case IOCFC_E_DISABLE:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+                break;
+
+        case IOCFC_E_STOP:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_stopping);
+                break;
+
         case IOCFC_E_IOC_FAILED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_failed);
                 break;
@@ -298,6 +317,15 @@ bfa_iocfc_sm_dconf_read(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
         case IOCFC_E_DCONF_DONE:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_cfg_wait);
                 break;
+
+        case IOCFC_E_DISABLE:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+                break;
+
+        case IOCFC_E_STOP:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_stopping);
+                break;
+
         case IOCFC_E_IOC_FAILED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_failed);
                 break;
@@ -322,6 +350,15 @@ bfa_iocfc_sm_init_cfg_wait(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
         case IOCFC_E_CFG_DONE:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_cfg_done);
                 break;
+
+        case IOCFC_E_DISABLE:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+                break;
+
+        case IOCFC_E_STOP:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_stopping);
+                break;
+
         case IOCFC_E_IOC_FAILED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_failed);
                 break;
@@ -433,6 +470,12 @@ bfa_iocfc_sm_stopping(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
                 bfa_cb_queue(iocfc->bfa, &iocfc->bfa->iocfc.stop_hcb_qe,
                              bfa_iocfc_stop_cb, iocfc->bfa);
                 break;
+
+        case IOCFC_E_IOC_ENABLED:
+        case IOCFC_E_DCONF_DONE:
+        case IOCFC_E_CFG_DONE:
+                break;
+
         default:
                 bfa_sm_fault(iocfc->bfa, event);
                 break;
@@ -454,6 +497,15 @@ bfa_iocfc_sm_enabling(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
         case IOCFC_E_IOC_ENABLED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_cfg_wait);
                 break;
+
+        case IOCFC_E_DISABLE:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+                break;
+
+        case IOCFC_E_STOP:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_dconf_write);
+                break;
+
         case IOCFC_E_IOC_FAILED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_failed);
 
@@ -493,6 +545,13 @@ bfa_iocfc_sm_cfg_wait(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
                              bfa_iocfc_enable_cb, iocfc->bfa);
                 iocfc->bfa->iocfc.cb_reqd = BFA_FALSE;
                 break;
+        case IOCFC_E_DISABLE:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+                break;
+
+        case IOCFC_E_STOP:
+                bfa_fsm_set_state(iocfc, bfa_iocfc_sm_dconf_write);
+                break;
         case IOCFC_E_IOC_FAILED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_failed);
                 if (iocfc->bfa->iocfc.cb_reqd == BFA_FALSE)
@@ -524,6 +583,10 @@ bfa_iocfc_sm_disabling(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
         case IOCFC_E_IOC_DISABLED:
                 bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabled);
                 break;
+        case IOCFC_E_IOC_ENABLED:
+        case IOCFC_E_DCONF_DONE:
+        case IOCFC_E_CFG_DONE:
+                break;
         default:
                 bfa_sm_fault(iocfc->bfa, event);
                 break;
@@ -785,19 +848,20 @@ void
 bfa_isr_enable(struct bfa_s *bfa)
 {
         u32 umsk;
-        int pci_func = bfa_ioc_pcifn(&bfa->ioc);
+        int port_id = bfa_ioc_portid(&bfa->ioc);
 
-        bfa_trc(bfa, pci_func);
+        bfa_trc(bfa, bfa_ioc_pcifn(&bfa->ioc));
+        bfa_trc(bfa, port_id);
 
         bfa_msix_ctrl_install(bfa);
 
         if (bfa_asic_id_ct2(bfa->ioc.pcidev.device_id)) {
                 umsk = __HFN_INT_ERR_MASK_CT2;
-                umsk |= pci_func == 0 ?
+                umsk |= port_id == 0 ?
                         __HFN_INT_FN0_MASK_CT2 : __HFN_INT_FN1_MASK_CT2;
         } else {
                 umsk = __HFN_INT_ERR_MASK;
-                umsk |= pci_func == 0 ? __HFN_INT_FN0_MASK : __HFN_INT_FN1_MASK;
+                umsk |= port_id == 0 ? __HFN_INT_FN0_MASK : __HFN_INT_FN1_MASK;
         }
 
         writel(umsk, bfa->iocfc.bfa_regs.intr_status);
@@ -930,7 +994,8 @@ bfa_iocfc_send_cfg(void *bfa_arg)
                 cfg_info->single_msix_vec = 1;
         cfg_info->endian_sig = BFI_IOC_ENDIAN_SIG;
         cfg_info->num_cqs = cfg->fwcfg.num_cqs;
-        cfg_info->num_ioim_reqs = cpu_to_be16(cfg->fwcfg.num_ioim_reqs);
+        cfg_info->num_ioim_reqs = cpu_to_be16(bfa_fcpim_get_throttle_cfg(bfa,
+                                               cfg->fwcfg.num_ioim_reqs));
         cfg_info->num_fwtio_reqs = cpu_to_be16(cfg->fwcfg.num_fwtio_reqs);
 
         bfa_dma_be_addr_set(cfg_info->cfgrsp_addr, iocfc->cfgrsp_dma.pa);
@@ -1192,10 +1257,14 @@ bfa_iocfc_qreg(struct bfa_s *bfa, struct bfi_iocfc_qreg_s *qreg)
 static void
 bfa_iocfc_res_recfg(struct bfa_s *bfa, struct bfa_iocfc_fwcfg_s *fwcfg)
 {
+        struct bfa_iocfc_s      *iocfc   = &bfa->iocfc;
+        struct bfi_iocfc_cfg_s  *cfg_info = iocfc->cfginfo;
+
         bfa_fcxp_res_recfg(bfa, fwcfg->num_fcxp_reqs);
         bfa_uf_res_recfg(bfa, fwcfg->num_uf_bufs);
         bfa_rport_res_recfg(bfa, fwcfg->num_rports);
-        bfa_fcp_res_recfg(bfa, fwcfg->num_ioim_reqs);
+        bfa_fcp_res_recfg(bfa, cpu_to_be16(cfg_info->num_ioim_reqs),
+                          fwcfg->num_ioim_reqs);
         bfa_tskim_res_recfg(bfa, fwcfg->num_tskim_reqs);
 }
 
@@ -1693,6 +1762,7 @@ bfa_cfg_get_meminfo(struct bfa_iocfc_cfg_s *cfg, struct bfa_meminfo_s *meminfo,
         struct bfa_mem_dma_s *flash_dma = BFA_MEM_FLASH_DMA(bfa);
         struct bfa_mem_dma_s *diag_dma = BFA_MEM_DIAG_DMA(bfa);
         struct bfa_mem_dma_s *phy_dma = BFA_MEM_PHY_DMA(bfa);
+        struct bfa_mem_dma_s *fru_dma = BFA_MEM_FRU_DMA(bfa);
 
         WARN_ON((cfg == NULL) || (meminfo == NULL));
 
@@ -1717,6 +1787,8 @@ bfa_cfg_get_meminfo(struct bfa_iocfc_cfg_s *cfg, struct bfa_meminfo_s *meminfo,
         bfa_mem_dma_setup(meminfo, diag_dma, bfa_diag_meminfo());
         bfa_mem_dma_setup(meminfo, phy_dma,
                           bfa_phy_meminfo(cfg->drvcfg.min_cfg));
+        bfa_mem_dma_setup(meminfo, fru_dma,
+                          bfa_fru_meminfo(cfg->drvcfg.min_cfg));
 }
 
 /*
@@ -1789,6 +1861,7 @@ bfa_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
         bfa_com_flash_attach(bfa, cfg->drvcfg.min_cfg);
         bfa_com_diag_attach(bfa);
         bfa_com_phy_attach(bfa, cfg->drvcfg.min_cfg);
+        bfa_com_fru_attach(bfa, cfg->drvcfg.min_cfg);
 }
 
 /*
diff --git a/drivers/scsi/bfa/bfa_defs.h b/drivers/scsi/bfa/bfa_defs.h
index b5a1595cc0a5..0efdf312b42c 100644
--- a/drivers/scsi/bfa/bfa_defs.h
+++ b/drivers/scsi/bfa/bfa_defs.h
@@ -159,10 +159,13 @@ enum bfa_status {
         BFA_STATUS_BEACON_ON    = 72,   /* Port Beacon already on */
         BFA_STATUS_ENOFSAVE     = 78,   /*  No saved firmware trace */
         BFA_STATUS_IOC_DISABLED = 82,   /* IOC is already disabled */
+        BFA_STATUS_ERROR_TRL_ENABLED  = 87,   /* TRL is enabled */
+        BFA_STATUS_ERROR_QOS_ENABLED  = 88,   /* QoS is enabled */
         BFA_STATUS_NO_SFP_DEV = 89,     /* No SFP device check or replace SFP */
         BFA_STATUS_MEMTEST_FAILED = 90, /* Memory test failed contact support */
         BFA_STATUS_LEDTEST_OP = 109, /* LED test is operating */
         BFA_STATUS_INVALID_MAC  = 134, /*  Invalid MAC address */
+        BFA_STATUS_CMD_NOTSUPP_CNA = 146, /* Command not supported for CNA */
         BFA_STATUS_PBC          = 154, /*  Operation not allowed for pre-boot
                                         *  configuration */
         BFA_STATUS_BAD_FWCFG = 156,     /* Bad firmware configuration */
@@ -184,6 +187,17 @@ enum bfa_status {
         BFA_STATUS_FAA_ACQ_ADDR = 200,  /* Acquiring addr */
         BFA_STATUS_ERROR_TRUNK_ENABLED = 203,   /* Trunk enabled on adapter */
         BFA_STATUS_MAX_ENTRY_REACHED = 212,     /* MAX entry reached */
+        BFA_STATUS_TOPOLOGY_LOOP = 230, /* Topology is set to Loop */
+        BFA_STATUS_LOOP_UNSUPP_MEZZ = 231, /* Loop topology is not supported
+                                            * on mezz cards */
+        BFA_STATUS_INVALID_BW = 233,    /* Invalid bandwidth value */
+        BFA_STATUS_QOS_BW_INVALID = 234,   /* Invalid QOS bandwidth
+                                            * configuration */
+        BFA_STATUS_DPORT_ENABLED = 235, /* D-port mode is already enabled */
+        BFA_STATUS_DPORT_DISABLED = 236, /* D-port mode is already disabled */
+        BFA_STATUS_CMD_NOTSUPP_MEZZ = 239, /* Cmd not supported for MEZZ card */
+        BFA_STATUS_FRU_NOT_PRESENT = 240, /* fru module not present */
+        BFA_STATUS_DPORT_ERR = 245,     /* D-port mode is enabled */
         BFA_STATUS_MAX_VAL              /* Unknown error code */
 };
 #define bfa_status_t enum bfa_status
@@ -249,6 +263,10 @@ struct bfa_adapter_attr_s {
 
         u8              is_mezz;
         u8              trunk_capable;
+        u8              mfg_day;        /* manufacturing day */
+        u8              mfg_month;      /* manufacturing month */
+        u16             mfg_year;       /* manufacturing year */
+        u16             rsvd;
 };
 
 /*
@@ -499,6 +517,17 @@ struct bfa_ioc_aen_data_s {
 };
 
 /*
+ *      D-port states
+ *
+*/
+enum bfa_dport_state {
+        BFA_DPORT_ST_DISABLED   = 0,    /* D-port is Disabled */
+        BFA_DPORT_ST_DISABLING  = 1,    /* D-port is Disabling */
+        BFA_DPORT_ST_ENABLING   = 2,    /* D-port is Enabling */
+        BFA_DPORT_ST_ENABLED    = 3,    /* D-port is Enabled */
+};
+
+/*
  * ---------------------- mfg definitions ------------
  */
 
@@ -722,7 +751,8 @@ struct bfa_ablk_cfg_pf_s {
         u8      rsvd[1];
         u16     num_qpairs;
         u16     num_vectors;
-        u32     bw;
+        u16     bw_min;
+        u16     bw_max;
 };
 
 struct bfa_ablk_cfg_port_s {
@@ -889,11 +919,40 @@ struct sfp_diag_ext_s {
         u8      ext_status_ctl[2];
 };
 
+/*
+ * Diagnostic: Data Fields -- Address A2h
+ * General Use Fields: User Writable Table - Features's Control Registers
+ * Total 32 bytes
+ */
+struct sfp_usr_eeprom_s {
+        u8      rsvd1[2];       /* 128-129 */
+        u8      ewrap;          /* 130 */
+        u8      rsvd2[2];       /*  */
+        u8      owrap;          /* 133 */
+        u8      rsvd3[2];       /*  */
+        u8      prbs;           /* 136: PRBS 7 generator */
+        u8      rsvd4[2];       /*  */
+        u8      tx_eqz_16;      /* 139: TX Equalizer (16xFC) */
+        u8      tx_eqz_8;       /* 140: TX Equalizer (8xFC) */
+        u8      rsvd5[2];       /*  */
+        u8      rx_emp_16;      /* 143: RX Emphasis (16xFC) */
+        u8      rx_emp_8;       /* 144: RX Emphasis (8xFC) */
+        u8      rsvd6[2];       /*  */
+        u8      tx_eye_adj;     /* 147: TX eye Threshold Adjust */
+        u8      rsvd7[3];       /*  */
+        u8      tx_eye_qctl;    /* 151: TX eye Quality Control */
+        u8      tx_eye_qres;    /* 152: TX eye Quality Result */
+        u8      rsvd8[2];       /*  */
+        u8      poh[3];         /* 155-157: Power On Hours */
+        u8      rsvd9[2];       /*  */
+};
+
 struct sfp_mem_s {
         struct sfp_srlid_base_s srlid_base;
         struct sfp_srlid_ext_s  srlid_ext;
         struct sfp_diag_base_s  diag_base;
         struct sfp_diag_ext_s   diag_ext;
+        struct sfp_usr_eeprom_s usr_eeprom;
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_defs_svc.h b/drivers/scsi/bfa/bfa_defs_svc.h
index 36756ce0e58f..ec03c8cd8dac 100644
--- a/drivers/scsi/bfa/bfa_defs_svc.h
+++ b/drivers/scsi/bfa/bfa_defs_svc.h
@@ -258,6 +258,7 @@ struct bfa_fw_port_lksm_stats_s {
         u32    hwsm_lrr_rx;        /*  No. of times LRR rx-ed by HWSM      */
         u32    hwsm_lr_rx;         /*  No. of times LR rx-ed by HWSM       */
         u32    bbsc_lr;            /* LKSM LR tx for credit recovery       */
+        u32     rsvd;
 };
 
 struct bfa_fw_port_snsm_stats_s {
@@ -270,6 +271,9 @@ struct bfa_fw_port_snsm_stats_s {
         u32    sync_lost;          /*  Sync loss count                     */
         u32    sig_lost;           /*  Signal loss count                   */
         u32    asn8g_attempts;     /* SNSM HWSM at 8Gbps attempts          */
+        u32    adapt_success;      /* SNSM adaptation success   */
+        u32    adapt_fails;        /* SNSM adaptation failures */
+        u32    adapt_ign_fails;    /* SNSM adaptation failures ignored */
 };
 
 struct bfa_fw_port_physm_stats_s {
@@ -324,12 +328,46 @@ struct bfa_fw_fcoe_port_stats_s {
         struct bfa_fw_fip_stats_s               fip_stats;
 };
 
+/**
+ * @brief LPSM statistics
+ */
+struct bfa_fw_lpsm_stats_s {
+        u32     cls_rx;         /* LPSM cls_rx                  */
+        u32     cls_tx;         /* LPSM cls_tx                  */
+        u32     arbf0_rx;       /* LPSM abrf0 rcvd              */
+        u32     arbf0_tx;       /* LPSM abrf0 xmit              */
+        u32     init_rx;        /* LPSM loop init start         */
+        u32     unexp_hwst;     /* LPSM unknown hw state        */
+        u32     unexp_frame;    /* LPSM unknown_frame           */
+        u32     unexp_prim;     /* LPSM unexpected primitive    */
+        u32     prev_alpa_unavail; /* LPSM prev alpa unavailable */
+        u32     alpa_unavail;   /* LPSM alpa not available      */
+        u32     lip_rx;         /* LPSM lip rcvd                */
+        u32     lip_f7f7_rx;    /* LPSM lip f7f7 rcvd           */
+        u32     lip_f8_rx;      /* LPSM lip f8 rcvd             */
+        u32     lip_f8f7_rx;    /* LPSM lip f8f7 rcvd           */
+        u32     lip_other_rx;   /* LPSM lip other rcvd          */
+        u32     lip_tx;         /* LPSM lip xmit                */
+        u32     retry_tov;      /* LPSM retry TOV               */
+        u32     lip_tov;        /* LPSM LIP wait TOV            */
+        u32     idle_tov;       /* LPSM idle wait TOV           */
+        u32     arbf0_tov;      /* LPSM arbfo wait TOV          */
+        u32     stop_loop_tov;  /* LPSM stop loop wait TOV      */
+        u32     lixa_tov;       /* LPSM lisa wait TOV           */
+        u32     lixx_tov;       /* LPSM lilp/lirp wait TOV      */
+        u32     cls_tov;        /* LPSM cls wait TOV            */
+        u32     sler;           /* LPSM SLER recvd              */
+        u32     failed;         /* LPSM failed                  */
+        u32     success;        /* LPSM online                  */
+};
+
 /*
  * IOC firmware FC uport stats
  */
 struct bfa_fw_fc_uport_stats_s {
         struct bfa_fw_port_snsm_stats_s         snsm_stats;
         struct bfa_fw_port_lksm_stats_s         lksm_stats;
+        struct bfa_fw_lpsm_stats_s              lpsm_stats;
 };
 
 /*
@@ -357,11 +395,6 @@ struct bfa_fw_fcxchg_stats_s {
         u32     ua_state_inv;
 };
 
-struct bfa_fw_lpsm_stats_s {
-        u32     cls_rx;
-        u32     cls_tx;
-};
-
 /*
  *  Trunk statistics
  */
@@ -454,7 +487,6 @@ struct bfa_fw_stats_s {
         struct bfa_fw_io_stats_s        io_stats;
         struct bfa_fw_port_stats_s      port_stats;
         struct bfa_fw_fcxchg_stats_s    fcxchg_stats;
-        struct bfa_fw_lpsm_stats_s      lpsm_stats;
         struct bfa_fw_lps_stats_s       lps_stats;
         struct bfa_fw_trunk_stats_s     trunk_stats;
         struct bfa_fw_advsm_stats_s     advsm_stats;
@@ -494,13 +526,23 @@ enum bfa_qos_bw_alloc {
         BFA_QOS_BW_LOW  =  10,  /*  bandwidth allocation for Low */
 };
 #pragma pack(1)
+
+struct bfa_qos_bw_s {
+        u8      qos_bw_set;
+        u8      high;
+        u8      med;
+        u8      low;
+};
+
 /*
  * QoS attribute returned in QoS Query
  */
 struct bfa_qos_attr_s {
-        u8              state;          /*  QoS current state */
-        u8              rsvd[3];
-        u32  total_bb_cr;               /*  Total BB Credits */
+        u8      state;          /*  QoS current state */
+        u8      rsvd1[3];
+        u32     total_bb_cr;    /*  Total BB Credits */
+        struct bfa_qos_bw_s qos_bw;     /* QOS bw cfg */
+        struct bfa_qos_bw_s qos_bw_op;  /* QOS bw operational */
 };
 
 /*
@@ -692,7 +734,8 @@ enum bfa_port_states {
         BFA_PORT_ST_FWMISMATCH          = 12,
         BFA_PORT_ST_PREBOOT_DISABLED    = 13,
         BFA_PORT_ST_TOGGLING_QWAIT      = 14,
-        BFA_PORT_ST_ACQ_ADDR            = 15,
+        BFA_PORT_ST_FAA_MISCONFIG       = 15,
+        BFA_PORT_ST_DPORT               = 16,
         BFA_PORT_ST_MAX_STATE,
 };
 
@@ -714,9 +757,11 @@ enum bfa_port_type {
  */
 enum bfa_port_topology {
         BFA_PORT_TOPOLOGY_NONE = 0,     /*  No valid topology */
-        BFA_PORT_TOPOLOGY_P2P  = 1,     /*  P2P only */
-        BFA_PORT_TOPOLOGY_LOOP = 2,     /*  LOOP topology */
-        BFA_PORT_TOPOLOGY_AUTO = 3,     /*  auto topology selection */
+        BFA_PORT_TOPOLOGY_P2P_OLD_VER = 1, /* P2P def for older ver */
+        BFA_PORT_TOPOLOGY_LOOP = 2,     /* LOOP topology */
+        BFA_PORT_TOPOLOGY_AUTO_OLD_VER = 3, /* auto def for older ver */
+        BFA_PORT_TOPOLOGY_AUTO = 4,     /* auto topology selection */
+        BFA_PORT_TOPOLOGY_P2P = 5,      /* P2P only */
 };
 
 /*
@@ -760,6 +805,7 @@ enum bfa_port_linkstate_rsn {
         BFA_PORT_LINKSTATE_RSN_LOCAL_FAULT      = 9,
         BFA_PORT_LINKSTATE_RSN_REMOTE_FAULT     = 10,
         BFA_PORT_LINKSTATE_RSN_TIMEOUT          = 11,
+        BFA_PORT_LINKSTATE_RSN_FAA_MISCONFIG    = 12,
 
 
 
@@ -833,6 +879,19 @@ struct bfa_lunmask_cfg_s {
         struct bfa_lun_mask_s   lun_list[MAX_LUN_MASK_CFG];
 };
 
+struct bfa_throttle_cfg_s {
+        u16     is_valid;
+        u16     value;
+        u32     rsvd;
+};
+
+struct bfa_defs_fcpim_throttle_s {
+        u16     max_value;
+        u16     cur_value;
+        u16     cfg_value;
+        u16     rsvd;
+};
+
 /*
  *      Physical port configuration
  */
@@ -851,9 +910,10 @@ struct bfa_port_cfg_s {
         u8       bb_scn;        /*  BB_SCN value from FLOGI Exchg */
         u8       bb_scn_state;  /*  Config state of BB_SCN */
         u8       faa_state;     /*  FAA enabled/disabled        */
-        u8       rsvd[1];
+        u8       rsvd1;
         u16      path_tov;      /*  device path timeout */
         u16      q_depth;       /*  SCSI Queue depth            */
+        struct bfa_qos_bw_s qos_bw;     /* QOS bandwidth        */
 };
 #pragma pack()
 
@@ -901,7 +961,7 @@ struct bfa_port_attr_s {
 
         /* FCoE specific  */
         u16                     fcoe_vlan;
-        u8                      rsvd1[2];
+        u8                      rsvd1[6];
 };
 
 /*
@@ -971,6 +1031,13 @@ struct bfa_trunk_vc_attr_s {
         u16 vc_credits[8];
 };
 
+struct bfa_fcport_loop_info_s {
+        u8      myalpa;         /* alpa claimed */
+        u8      alpabm_val;     /* alpa bitmap valid or not (1 or 0) */
+        u8      resvd[6];
+        struct fc_alpabm_s alpabm;      /* alpa bitmap */
+};
+
 /*
  *      Link state information
  */
@@ -981,13 +1048,18 @@ struct bfa_port_link_s {
         u8       speed;         /*  Link speed (1/2/4/8 G) */
         u32      linkstate_opt; /*  Linkstate optional data (debug) */
         u8       trunked;       /*  Trunked or not (1 or 0) */
-        u8       resvd[3];
+        u8       resvd[7];
         struct bfa_qos_attr_s  qos_attr;   /* QoS Attributes */
         union {
-                struct bfa_qos_vc_attr_s qos_vc_attr;  /*  VC info from ELP */
-                struct bfa_trunk_vc_attr_s trunk_vc_attr;
-                struct bfa_fcport_fcf_s fcf; /*  FCF information (for FCoE) */
-        } vc_fcf;
+                struct bfa_fcport_loop_info_s loop_info;
+                union {
+                        struct bfa_qos_vc_attr_s qos_vc_attr;
+                                        /*  VC info from ELP */
+                        struct bfa_trunk_vc_attr_s trunk_vc_attr;
+                        struct bfa_fcport_fcf_s fcf;
+                                        /*  FCF information (for FCoE) */
+                } vc_fcf;
+        } attr;
 };
 #pragma pack()
 
@@ -1112,6 +1184,9 @@ struct bfa_port_fc_stats_s {
         u64     tx_frames;      /*  Tx frames                   */
         u64     tx_words;       /*  Tx words                    */
         u64     tx_lip;         /*  Tx LIP                      */
+        u64     tx_lip_f7f7;    /*  Tx LIP_F7F7         */
+        u64     tx_lip_f8f7;    /*  Tx LIP_F8F7         */
+        u64     tx_arbf0;       /*  Tx ARB F0                   */
         u64     tx_nos;         /*  Tx NOS                      */
         u64     tx_ols;         /*  Tx OLS                      */
         u64     tx_lr;          /*  Tx LR                       */
@@ -1119,6 +1194,9 @@ struct bfa_port_fc_stats_s {
         u64     rx_frames;      /*  Rx frames                   */
         u64     rx_words;       /*  Rx words                    */
         u64     lip_count;      /*  Rx LIP                      */
+        u64     rx_lip_f7f7;    /*  Rx LIP_F7F7         */
+        u64     rx_lip_f8f7;    /*  Rx LIP_F8F7         */
+        u64     rx_arbf0;       /*  Rx ARB F0                   */
         u64     nos_count;      /*  Rx NOS                      */
         u64     ols_count;      /*  Rx OLS                      */
         u64     lr_count;       /*  Rx LR                       */
@@ -1140,6 +1218,7 @@ struct bfa_port_fc_stats_s {
         u64     bbsc_frames_lost; /* Credit Recovery-Frames Lost  */
         u64     bbsc_credits_lost; /* Credit Recovery-Credits Lost */
         u64     bbsc_link_resets; /* Credit Recovery-Link Resets   */
+        u64     loop_timeouts;  /*  Loop timeouts               */
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_fc.h b/drivers/scsi/bfa/bfa_fc.h
index e0beb4d7e264..bea821b98030 100644
--- a/drivers/scsi/bfa/bfa_fc.h
+++ b/drivers/scsi/bfa/bfa_fc.h
@@ -24,6 +24,7 @@ typedef u64 wwn_t;
 
 #define WWN_NULL        (0)
 #define FC_SYMNAME_MAX  256     /*  max name server symbolic name size */
+#define FC_ALPA_MAX     128
 
 #pragma pack(1)
 
@@ -1015,6 +1016,10 @@ struct fc_symname_s {
         u8         symname[FC_SYMNAME_MAX];
 };
 
+struct fc_alpabm_s {
+        u8      alpa_bm[FC_ALPA_MAX / 8];
+};
+
 /*
  * protocol default timeout values
  */
diff --git a/drivers/scsi/bfa/bfa_fcbuild.c b/drivers/scsi/bfa/bfa_fcbuild.c
index 273cee90b3b4..dce787f6cca2 100644
--- a/drivers/scsi/bfa/bfa_fcbuild.c
+++ b/drivers/scsi/bfa/bfa_fcbuild.c
@@ -228,6 +228,10 @@ fc_plogi_x_build(struct fchs_s *fchs, void *pld, u32 d_id, u32 s_id,
 
         memcpy(plogi, &plogi_tmpl, sizeof(struct fc_logi_s));
 
+        /* For FC AL bb_cr is 0 and altbbcred is 1 */
+        if (!bb_cr)
+                plogi->csp.altbbcred = 1;
+
         plogi->els_cmd.els_code = els_code;
         if (els_code == FC_ELS_PLOGI)
                 fc_els_req_build(fchs, d_id, s_id, ox_id);
diff --git a/drivers/scsi/bfa/bfa_fcpim.c b/drivers/scsi/bfa/bfa_fcpim.c
index 1633963c66ca..27b560962357 100644
--- a/drivers/scsi/bfa/bfa_fcpim.c
+++ b/drivers/scsi/bfa/bfa_fcpim.c
@@ -158,6 +158,7 @@ enum bfa_tskim_event {
         BFA_TSKIM_SM_IOS_DONE   = 7,    /*  IO and sub TM completions   */
         BFA_TSKIM_SM_CLEANUP    = 8,    /*  TM cleanup on ITN offline   */
         BFA_TSKIM_SM_CLEANUP_DONE = 9,  /*  TM abort completion */
+        BFA_TSKIM_SM_UTAG       = 10,   /*  TM completion unknown tag  */
 };
 
 /*
@@ -3036,7 +3037,7 @@ bfa_ioim_abort(struct bfa_ioim_s *ioim)
 static void
 bfa_tskim_sm_uninit(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_START:
@@ -3074,7 +3075,7 @@ bfa_tskim_sm_uninit(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_active(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_DONE:
@@ -3110,7 +3111,7 @@ bfa_tskim_sm_active(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_cleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_DONE:
@@ -3119,6 +3120,7 @@ bfa_tskim_sm_cleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
                  */
                 break;
 
+        case BFA_TSKIM_SM_UTAG:
         case BFA_TSKIM_SM_CLEANUP_DONE:
                 bfa_sm_set_state(tskim, bfa_tskim_sm_iocleanup);
                 bfa_tskim_cleanup_ios(tskim);
@@ -3138,7 +3140,7 @@ bfa_tskim_sm_cleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_iocleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_IOS_DONE:
@@ -3170,7 +3172,7 @@ bfa_tskim_sm_iocleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_qfull(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_QRESUME:
@@ -3207,7 +3209,7 @@ static void
 bfa_tskim_sm_cleanup_qfull(struct bfa_tskim_s *tskim,
                 enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_DONE:
@@ -3238,7 +3240,7 @@ bfa_tskim_sm_cleanup_qfull(struct bfa_tskim_s *tskim,
 static void
 bfa_tskim_sm_hcb(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-        bfa_trc(tskim->bfa, event);
+        bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
         switch (event) {
         case BFA_TSKIM_SM_HCB:
@@ -3560,6 +3562,8 @@ bfa_tskim_isr(struct bfa_s *bfa, struct bfi_msg_s *m)
         if (rsp->tsk_status == BFI_TSKIM_STS_ABORTED) {
                 bfa_stats(tskim->itnim, tm_cleanup_comps);
                 bfa_sm_send_event(tskim, BFA_TSKIM_SM_CLEANUP_DONE);
+        } else if (rsp->tsk_status == BFI_TSKIM_STS_UTAG) {
+                bfa_sm_send_event(tskim, BFA_TSKIM_SM_UTAG);
         } else {
                 bfa_stats(tskim->itnim, tm_fw_rsps);
                 bfa_sm_send_event(tskim, BFA_TSKIM_SM_DONE);
@@ -3699,6 +3703,7 @@ bfa_fcp_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
         struct bfa_mem_dma_s *seg_ptr;
         u16     idx, nsegs, num_io_req;
 
+        fcp->max_ioim_reqs = cfg->fwcfg.num_ioim_reqs;
         fcp->num_ioim_reqs = cfg->fwcfg.num_ioim_reqs;
         fcp->num_fwtio_reqs  = cfg->fwcfg.num_fwtio_reqs;
         fcp->num_itns   = cfg->fwcfg.num_rports;
@@ -3721,6 +3726,7 @@ bfa_fcp_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
                 bfa_iocfc_set_snsbase(bfa, idx, fcp->snsbase[idx].pa);
         }
 
+        fcp->throttle_update_required = 1;
         bfa_fcpim_attach(fcp, bfad, cfg, pcidev);
 
         bfa_iotag_attach(fcp);
@@ -3759,23 +3765,33 @@ bfa_fcp_iocdisable(struct bfa_s *bfa)
 {
         struct bfa_fcp_mod_s *fcp = BFA_FCP_MOD(bfa);
 
-        /* Enqueue unused ioim resources to free_q */
-        list_splice_tail_init(&fcp->iotag_unused_q, &fcp->iotag_ioim_free_q);
-
         bfa_fcpim_iocdisable(fcp);
 }
 
 void
-bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw)
+bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw, u16 max_ioim_fw)
 {
         struct bfa_fcp_mod_s    *mod = BFA_FCP_MOD(bfa);
         struct list_head        *qe;
         int     i;
 
+        /* Update io throttle value only once during driver load time */
+        if (!mod->throttle_update_required)
+                return;
+
         for (i = 0; i < (mod->num_ioim_reqs - num_ioim_fw); i++) {
                 bfa_q_deq_tail(&mod->iotag_ioim_free_q, &qe);
                 list_add_tail(qe, &mod->iotag_unused_q);
         }
+
+        if (mod->num_ioim_reqs != num_ioim_fw) {
+                bfa_trc(bfa, mod->num_ioim_reqs);
+                bfa_trc(bfa, num_ioim_fw);
+        }
+
+        mod->max_ioim_reqs = max_ioim_fw;
+        mod->num_ioim_reqs = num_ioim_fw;
+        mod->throttle_update_required = 0;
 }
 
 void
@@ -3833,3 +3849,88 @@ bfa_iotag_attach(struct bfa_fcp_mod_s *fcp)
 
         bfa_mem_kva_curp(fcp) = (u8 *) iotag;
 }
+
+
+/**
+ * To send config req, first try to use throttle value from flash
+ * If 0, then use driver parameter
+ * We need to use min(flash_val, drv_val) because
+ * memory allocation was done based on this cfg'd value
+ */
+u16
+bfa_fcpim_get_throttle_cfg(struct bfa_s *bfa, u16 drv_cfg_param)
+{
+        u16 tmp;
+        struct bfa_fcp_mod_s *fcp = BFA_FCP_MOD(bfa);
+
+        /*
+         * If throttle value from flash is already in effect after driver is
+         * loaded then until next load, always return current value instead
+         * of actual flash value
+         */
+        if (!fcp->throttle_update_required)
+                return (u16)fcp->num_ioim_reqs;
+
+        tmp = bfa_dconf_read_data_valid(bfa) ? bfa_fcpim_read_throttle(bfa) : 0;
+        if (!tmp || (tmp > drv_cfg_param))
+                tmp = drv_cfg_param;
+
+        return tmp;
+}
+
+bfa_status_t
+bfa_fcpim_write_throttle(struct bfa_s *bfa, u16 value)
+{
+        if (!bfa_dconf_get_min_cfg(bfa)) {
+                BFA_DCONF_MOD(bfa)->dconf->throttle_cfg.value = value;
+                BFA_DCONF_MOD(bfa)->dconf->throttle_cfg.is_valid = 1;
+                return BFA_STATUS_OK;
+        }
+
+        return BFA_STATUS_FAILED;
+}
+
+u16
+bfa_fcpim_read_throttle(struct bfa_s *bfa)
+{
+        struct bfa_throttle_cfg_s *throttle_cfg =
+                        &(BFA_DCONF_MOD(bfa)->dconf->throttle_cfg);
+
+        return ((!bfa_dconf_get_min_cfg(bfa)) ?
+               ((throttle_cfg->is_valid == 1) ? (throttle_cfg->value) : 0) : 0);
+}
+
+bfa_status_t
+bfa_fcpim_throttle_set(struct bfa_s *bfa, u16 value)
+{
+        /* in min cfg no commands should run. */
+        if ((bfa_dconf_get_min_cfg(bfa) == BFA_TRUE) ||
+            (!bfa_dconf_read_data_valid(bfa)))
+                return BFA_STATUS_FAILED;
+
+        bfa_fcpim_write_throttle(bfa, value);
+
+        return bfa_dconf_update(bfa);
+}
+
+bfa_status_t
+bfa_fcpim_throttle_get(struct bfa_s *bfa, void *buf)
+{
+        struct bfa_fcpim_s *fcpim = BFA_FCPIM(bfa);
+        struct bfa_defs_fcpim_throttle_s throttle;
+
+        if ((bfa_dconf_get_min_cfg(bfa) == BFA_TRUE) ||
+            (!bfa_dconf_read_data_valid(bfa)))
+                return BFA_STATUS_FAILED;
+
+        memset(&throttle, 0, sizeof(struct bfa_defs_fcpim_throttle_s));
+
+        throttle.cur_value = (u16)(fcpim->fcp->num_ioim_reqs);
+        throttle.cfg_value = bfa_fcpim_read_throttle(bfa);
+        if (!throttle.cfg_value)
+                throttle.cfg_value = throttle.cur_value;
+        throttle.max_value = (u16)(fcpim->fcp->max_ioim_reqs);
+        memcpy(buf, &throttle, sizeof(struct bfa_defs_fcpim_throttle_s));
+
+        return BFA_STATUS_OK;
+}
diff --git a/drivers/scsi/bfa/bfa_fcpim.h b/drivers/scsi/bfa/bfa_fcpim.h
index 36f26da80f76..e693af6e5930 100644
--- a/drivers/scsi/bfa/bfa_fcpim.h
+++ b/drivers/scsi/bfa/bfa_fcpim.h
@@ -42,7 +42,7 @@ void bfa_itn_create(struct bfa_s *bfa, struct bfa_rport_s *rport,
                 void (*isr)(struct bfa_s *bfa, struct bfi_msg_s *m));
 void bfa_itn_isr(struct bfa_s *bfa, struct bfi_msg_s *m);
 void bfa_iotag_attach(struct bfa_fcp_mod_s *fcp);
-void bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw);
+void bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw, u16 max_ioim_fw);
 
 #define BFA_FCP_MOD(_hal)       (&(_hal)->modules.fcp_mod)
 #define BFA_MEM_FCP_KVA(__bfa)  (&(BFA_FCP_MOD(__bfa)->kva_seg))
@@ -51,7 +51,9 @@ void bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw);
 #define BFA_ITN_FROM_TAG(_fcp, _tag)    \
         ((_fcp)->itn_arr + ((_tag) & ((_fcp)->num_itns - 1)))
 #define BFA_SNSINFO_FROM_TAG(_fcp, _tag) \
-        bfa_mem_get_dmabuf_kva(_fcp, _tag, BFI_IOIM_SNSLEN)
+        bfa_mem_get_dmabuf_kva(_fcp, (_tag & BFA_IOIM_IOTAG_MASK),      \
+        BFI_IOIM_SNSLEN)
+
 
 #define BFA_ITNIM_MIN   32
 #define BFA_ITNIM_MAX   1024
@@ -148,6 +150,7 @@ struct bfa_fcp_mod_s {
         struct list_head        iotag_unused_q; /* unused IO resources*/
         struct bfa_iotag_s      *iotag_arr;
         struct bfa_itn_s        *itn_arr;
+        int                     max_ioim_reqs;
         int                     num_ioim_reqs;
         int                     num_fwtio_reqs;
         int                     num_itns;
@@ -155,6 +158,7 @@ struct bfa_fcp_mod_s {
         struct bfa_fcpim_s      fcpim;
         struct bfa_mem_dma_s    dma_seg[BFA_FCP_DMA_SEGS];
         struct bfa_mem_kva_s    kva_seg;
+        int                     throttle_update_required;
 };
 
 /*
@@ -416,5 +420,10 @@ bfa_status_t	bfa_fcpim_lunmask_delete(struct bfa_s *bfa, u16 vf_id,
 bfa_status_t    bfa_fcpim_lunmask_add(struct bfa_s *bfa, u16 vf_id,
                                 wwn_t *pwwn, wwn_t rpwwn, struct scsi_lun lun);
 bfa_status_t    bfa_fcpim_lunmask_clear(struct bfa_s *bfa);
+u16             bfa_fcpim_read_throttle(struct bfa_s *bfa);
+bfa_status_t    bfa_fcpim_write_throttle(struct bfa_s *bfa, u16 value);
+bfa_status_t    bfa_fcpim_throttle_set(struct bfa_s *bfa, u16 value);
+bfa_status_t    bfa_fcpim_throttle_get(struct bfa_s *bfa, void *buf);
+u16     bfa_fcpim_get_throttle_cfg(struct bfa_s *bfa, u16 drv_cfg_param);
 
 #endif /* __BFA_FCPIM_H__ */
diff --git a/drivers/scsi/bfa/bfa_fcs.c b/drivers/scsi/bfa/bfa_fcs.c
index fd3e84d32bd2..d428808fb37e 100644
--- a/drivers/scsi/bfa/bfa_fcs.c
+++ b/drivers/scsi/bfa/bfa_fcs.c
@@ -303,16 +303,30 @@ static void
 bfa_fcs_fabric_sm_created(struct bfa_fcs_fabric_s *fabric,
                           enum bfa_fcs_fabric_event event)
 {
+        struct bfa_s    *bfa = fabric->fcs->bfa;
+
         bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
         bfa_trc(fabric->fcs, event);
 
         switch (event) {
         case BFA_FCS_FABRIC_SM_START:
-                if (bfa_fcport_is_linkup(fabric->fcs->bfa)) {
+                if (!bfa_fcport_is_linkup(fabric->fcs->bfa)) {
+                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_linkdown);
+                        break;
+                }
+                if (bfa_fcport_get_topology(bfa) ==
+                                BFA_PORT_TOPOLOGY_LOOP) {
+                        fabric->fab_type = BFA_FCS_FABRIC_LOOP;
+                        fabric->bport.pid = bfa_fcport_get_myalpa(bfa);
+                        fabric->bport.pid = bfa_hton3b(fabric->bport.pid);
+                        bfa_sm_set_state(fabric,
+                                        bfa_fcs_fabric_sm_online);
+                        bfa_fcs_fabric_set_opertype(fabric);
+                        bfa_fcs_lport_online(&fabric->bport);
+                } else {
                         bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_flogi);
                         bfa_fcs_fabric_login(fabric);
-                } else
-                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_linkdown);
+                }
                 break;
 
         case BFA_FCS_FABRIC_SM_LINK_UP:
@@ -337,16 +351,28 @@ static void
 bfa_fcs_fabric_sm_linkdown(struct bfa_fcs_fabric_s *fabric,
                            enum bfa_fcs_fabric_event event)
 {
+        struct bfa_s    *bfa = fabric->fcs->bfa;
+
         bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
         bfa_trc(fabric->fcs, event);
 
         switch (event) {
         case BFA_FCS_FABRIC_SM_LINK_UP:
-                bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_flogi);
-                bfa_fcs_fabric_login(fabric);
+                if (bfa_fcport_get_topology(bfa) != BFA_PORT_TOPOLOGY_LOOP) {
+                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_flogi);
+                        bfa_fcs_fabric_login(fabric);
+                        break;
+                }
+                fabric->fab_type = BFA_FCS_FABRIC_LOOP;
+                fabric->bport.pid = bfa_fcport_get_myalpa(bfa);
+                fabric->bport.pid = bfa_hton3b(fabric->bport.pid);
+                bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_online);
+                bfa_fcs_fabric_set_opertype(fabric);
+                bfa_fcs_lport_online(&fabric->bport);
                 break;
 
         case BFA_FCS_FABRIC_SM_RETRY_OP:
+        case BFA_FCS_FABRIC_SM_LOOPBACK:
                 break;
 
         case BFA_FCS_FABRIC_SM_DELETE:
@@ -595,14 +621,20 @@ void
 bfa_fcs_fabric_sm_online(struct bfa_fcs_fabric_s *fabric,
                          enum bfa_fcs_fabric_event event)
 {
+        struct bfa_s    *bfa = fabric->fcs->bfa;
+
         bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
         bfa_trc(fabric->fcs, event);
 
         switch (event) {
         case BFA_FCS_FABRIC_SM_LINK_DOWN:
                 bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_linkdown);
-                bfa_sm_send_event(fabric->lps, BFA_LPS_SM_OFFLINE);
-                bfa_fcs_fabric_notify_offline(fabric);
+                if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP) {
+                        bfa_fcs_lport_offline(&fabric->bport);
+                } else {
+                        bfa_sm_send_event(fabric->lps, BFA_LPS_SM_OFFLINE);
+                        bfa_fcs_fabric_notify_offline(fabric);
+                }
                 break;
 
         case BFA_FCS_FABRIC_SM_DELETE:
@@ -719,20 +751,29 @@ static void
 bfa_fcs_fabric_sm_stopping(struct bfa_fcs_fabric_s *fabric,
                            enum bfa_fcs_fabric_event event)
 {
+        struct bfa_s    *bfa = fabric->fcs->bfa;
+
         bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
         bfa_trc(fabric->fcs, event);
 
         switch (event) {
         case BFA_FCS_FABRIC_SM_STOPCOMP:
-                bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
-                bfa_sm_send_event(fabric->lps, BFA_LPS_SM_LOGOUT);
+                if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP) {
+                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_created);
+                } else {
+                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
+                        bfa_sm_send_event(fabric->lps, BFA_LPS_SM_LOGOUT);
+                }
                 break;
 
         case BFA_FCS_FABRIC_SM_LINK_UP:
                 break;
 
         case BFA_FCS_FABRIC_SM_LINK_DOWN:
-                bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
+                if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP)
+                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_created);
+                else
+                        bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
                 break;
 
         default:
@@ -975,9 +1016,6 @@ bfa_fcs_fabric_login(struct bfa_fcs_fabric_s *fabric)
         struct bfa_lport_cfg_s  *pcfg = &fabric->bport.port_cfg;
         u8                      alpa = 0, bb_scn = 0;
 
-        if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP)
-                alpa = bfa_fcport_get_myalpa(bfa);
-
         if (bfa_fcs_fabric_is_bbscn_enabled(fabric) &&
             (!fabric->fcs->bbscn_flogi_rjt))
                 bb_scn = BFA_FCS_PORT_DEF_BB_SCN;
diff --git a/drivers/scsi/bfa/bfa_fcs.h b/drivers/scsi/bfa/bfa_fcs.h
index 6c4377cb287f..a449706c6bc0 100644
--- a/drivers/scsi/bfa/bfa_fcs.h
+++ b/drivers/scsi/bfa/bfa_fcs.h
@@ -118,9 +118,9 @@ struct bfa_fcs_lport_fab_s {
 #define MAX_ALPA_COUNT  127
 
 struct bfa_fcs_lport_loop_s {
-        u8         num_alpa;    /*  Num of ALPA entries in the map */
-        u8         alpa_pos_map[MAX_ALPA_COUNT];        /*  ALPA Positional
-                                                         *Map */
+        u8      num_alpa;       /*  Num of ALPA entries in the map */
+        u8      alpabm_valid;   /* alpa bitmap valid or not (1 or 0) */
+        u8      alpa_pos_map[MAX_ALPA_COUNT]; /*  ALPA Positional Map */
         struct bfa_fcs_lport_s *port;   /*  parent port */
 };
 
@@ -175,6 +175,7 @@ enum bfa_fcs_fabric_type {
         BFA_FCS_FABRIC_UNKNOWN = 0,
         BFA_FCS_FABRIC_SWITCHED = 1,
         BFA_FCS_FABRIC_N2N = 2,
+        BFA_FCS_FABRIC_LOOP = 3,
 };
 
 
@@ -350,9 +351,10 @@ void		bfa_fcs_lport_ns_util_send_rspn_id(void *cbarg,
                                 struct bfa_fcxp_s *fcxp_alloced);
 void            bfa_fcs_lport_scn_init(struct bfa_fcs_lport_s *vport);
 void            bfa_fcs_lport_scn_offline(struct bfa_fcs_lport_s *vport);
-void            bfa_fcs_lport_scn_online(struct bfa_fcs_lport_s *vport);
+void            bfa_fcs_lport_fab_scn_online(struct bfa_fcs_lport_s *vport);
 void            bfa_fcs_lport_scn_process_rscn(struct bfa_fcs_lport_s *port,
                                               struct fchs_s *rx_frame, u32 len);
+void            bfa_fcs_lport_lip_scn_online(bfa_fcs_lport_t *port);
 
 struct bfa_fcs_vport_s {
         struct list_head                qe;             /*  queue elem  */
@@ -453,6 +455,7 @@ struct bfa_fcs_rport_s {
         struct bfa_rport_stats_s stats; /*  rport stats */
         enum bfa_rport_function scsi_function;  /*  Initiator/Target */
         struct bfa_fcs_rpf_s rpf;       /* Rport features module */
+        bfa_boolean_t   scn_online;     /* SCN online flag */
 };
 
 static inline struct bfa_rport_s *
@@ -639,9 +642,9 @@ struct bfa_fcs_fdmi_hba_attr_s {
         u8         model[16];
         u8         model_desc[256];
         u8         hw_version[8];
-        u8         driver_version[8];
+        u8         driver_version[BFA_VERSION_LEN];
         u8         option_rom_ver[BFA_VERSION_LEN];
-        u8         fw_version[8];
+        u8         fw_version[BFA_VERSION_LEN];
         u8         os_name[256];
         __be32        max_ct_pyld;
 };
@@ -733,7 +736,7 @@ enum rport_event {
         RPSM_EVENT_LOGO_IMP     = 5,    /*  implicit logo for SLER      */
         RPSM_EVENT_FCXP_SENT    = 6,    /*  Frame from has been sent    */
         RPSM_EVENT_DELETE       = 7,    /*  RPORT delete request        */
-        RPSM_EVENT_SCN          = 8,    /*  state change notification   */
+        RPSM_EVENT_FAB_SCN      = 8,    /*  state change notification   */
         RPSM_EVENT_ACCEPTED     = 9,    /*  Good response from remote device */
         RPSM_EVENT_FAILED       = 10,   /*  Request to rport failed.    */
         RPSM_EVENT_TIMEOUT      = 11,   /*  Rport SM timeout event      */
@@ -744,7 +747,9 @@ enum rport_event {
         RPSM_EVENT_ADDRESS_DISC = 16,   /*  Need to Discover rport's PID */
         RPSM_EVENT_PRLO_RCVD   = 17,    /*  PRLO from remote device     */
         RPSM_EVENT_PLOGI_RETRY = 18,    /*  Retry PLOGI continuously */
-        RPSM_EVENT_FC4_FCS_ONLINE = 19, /*!< FC-4 FCS online complete */
+        RPSM_EVENT_SCN_OFFLINE = 19,    /* loop scn offline             */
+        RPSM_EVENT_SCN_ONLINE   = 20,   /* loop scn online              */
+        RPSM_EVENT_FC4_FCS_ONLINE = 21, /* FC-4 FCS online complete */
 };
 
 /*
@@ -763,7 +768,7 @@ enum bfa_fcs_itnim_event {
         BFA_FCS_ITNIM_SM_DELETE = 10,   /*  delete event from rport */
         BFA_FCS_ITNIM_SM_PRLO = 11,     /*  delete event from rport */
         BFA_FCS_ITNIM_SM_RSP_NOT_SUPP = 12, /* cmd not supported rsp */
-        BFA_FCS_ITNIM_SM_HAL_ONLINE = 13, /*!< bfa rport online event */
+        BFA_FCS_ITNIM_SM_HAL_ONLINE = 13, /* bfa rport online event */
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_fcs_lport.c b/drivers/scsi/bfa/bfa_fcs_lport.c
index 3b75f6fb2de1..1224d0462a49 100644
--- a/drivers/scsi/bfa/bfa_fcs_lport.c
+++ b/drivers/scsi/bfa/bfa_fcs_lport.c
@@ -23,6 +23,34 @@
 
 BFA_TRC_FILE(FCS, PORT);
 
+/*
+ * ALPA to LIXA bitmap mapping
+ *
+ * ALPA 0x00 (Word 0, Bit 30) is invalid for N_Ports. Also Word 0 Bit 31
+ * is for L_bit (login required) and is filled as ALPA 0x00 here.
+ */
+static const u8 loop_alpa_map[] = {
+        0x00, 0x00, 0x01, 0x02, 0x04, 0x08, 0x0F, 0x10, /* Word 0 Bits 31..24 */
+        0x17, 0x18, 0x1B, 0x1D, 0x1E, 0x1F, 0x23, 0x25, /* Word 0 Bits 23..16 */
+        0x26, 0x27, 0x29, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E, /* Word 0 Bits 15..08 */
+        0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x39, 0x3A, /* Word 0 Bits 07..00 */
+
+        0x3C, 0x43, 0x45, 0x46, 0x47, 0x49, 0x4A, 0x4B, /* Word 1 Bits 31..24 */
+        0x4C, 0x4D, 0x4E, 0x51, 0x52, 0x53, 0x54, 0x55, /* Word 1 Bits 23..16 */
+        0x56, 0x59, 0x5A, 0x5C, 0x63, 0x65, 0x66, 0x67, /* Word 1 Bits 15..08 */
+        0x69, 0x6A, 0x6B, 0x6C, 0x6D, 0x6E, 0x71, 0x72, /* Word 1 Bits 07..00 */
+
+        0x73, 0x74, 0x75, 0x76, 0x79, 0x7A, 0x7C, 0x80, /* Word 2 Bits 31..24 */
+        0x81, 0x82, 0x84, 0x88, 0x8F, 0x90, 0x97, 0x98, /* Word 2 Bits 23..16 */
+        0x9B, 0x9D, 0x9E, 0x9F, 0xA3, 0xA5, 0xA6, 0xA7, /* Word 2 Bits 15..08 */
+        0xA9, 0xAA, 0xAB, 0xAC, 0xAD, 0xAE, 0xB1, 0xB2, /* Word 2 Bits 07..00 */
+
+        0xB3, 0xB4, 0xB5, 0xB6, 0xB9, 0xBA, 0xBC, 0xC3, /* Word 3 Bits 31..24 */
+        0xC5, 0xC6, 0xC7, 0xC9, 0xCA, 0xCB, 0xCC, 0xCD, /* Word 3 Bits 23..16 */
+        0xCE, 0xD1, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD9, /* Word 3 Bits 15..08 */
+        0xDA, 0xDC, 0xE0, 0xE1, 0xE2, 0xE4, 0xE8, 0xEF, /* Word 3 Bits 07..00 */
+};
+
 static void     bfa_fcs_lport_send_ls_rjt(struct bfa_fcs_lport_s *port,
                                          struct fchs_s *rx_fchs, u8 reason_code,
                                          u8 reason_code_expl);
@@ -51,6 +79,10 @@ static void	bfa_fcs_lport_n2n_init(struct bfa_fcs_lport_s *port);
 static void     bfa_fcs_lport_n2n_online(struct bfa_fcs_lport_s *port);
 static void     bfa_fcs_lport_n2n_offline(struct bfa_fcs_lport_s *port);
 
+static void     bfa_fcs_lport_loop_init(struct bfa_fcs_lport_s *port);
+static void     bfa_fcs_lport_loop_online(struct bfa_fcs_lport_s *port);
+static void     bfa_fcs_lport_loop_offline(struct bfa_fcs_lport_s *port);
+
 static struct {
         void            (*init) (struct bfa_fcs_lport_s *port);
         void            (*online) (struct bfa_fcs_lport_s *port);
@@ -62,7 +94,9 @@ static struct {
         bfa_fcs_lport_fab_init, bfa_fcs_lport_fab_online,
                         bfa_fcs_lport_fab_offline}, {
         bfa_fcs_lport_n2n_init, bfa_fcs_lport_n2n_online,
-                        bfa_fcs_lport_n2n_offline},
+                        bfa_fcs_lport_n2n_offline}, {
+        bfa_fcs_lport_loop_init, bfa_fcs_lport_loop_online,
+                        bfa_fcs_lport_loop_offline},
         };
 
 /*
@@ -1127,7 +1161,7 @@ static void
 bfa_fcs_lport_fab_online(struct bfa_fcs_lport_s *port)
 {
         bfa_fcs_lport_ns_online(port);
-        bfa_fcs_lport_scn_online(port);
+        bfa_fcs_lport_fab_scn_online(port);
 }
 
 /*
@@ -1221,6 +1255,98 @@ bfa_fcs_lport_n2n_offline(struct bfa_fcs_lport_s *port)
         n2n_port->reply_oxid = 0;
 }
 
+void
+bfa_fcport_get_loop_attr(struct bfa_fcs_lport_s *port)
+{
+        int i = 0, j = 0, bit = 0, alpa_bit = 0;
+        u8 k = 0;
+        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(port->fcs->bfa);
+
+        port->port_topo.ploop.alpabm_valid = fcport->alpabm_valid;
+        port->pid = fcport->myalpa;
+        port->pid = bfa_hton3b(port->pid);
+
+        for (i = 0; i < (FC_ALPA_MAX / 8); i++) {
+                for (j = 0, alpa_bit = 0; j < 8; j++, alpa_bit++) {
+                        bfa_trc(port->fcs->bfa, fcport->alpabm.alpa_bm[i]);
+                        bit = (fcport->alpabm.alpa_bm[i] & (1 << (7 - j)));
+                        if (bit) {
+                                port->port_topo.ploop.alpa_pos_map[k] =
+                                        loop_alpa_map[(i * 8) + alpa_bit];
+                                k++;
+                                bfa_trc(port->fcs->bfa, k);
+                                bfa_trc(port->fcs->bfa,
+                                         port->port_topo.ploop.alpa_pos_map[k]);
+                        }
+                }
+        }
+        port->port_topo.ploop.num_alpa = k;
+}
+
+/*
+ * Called by fcs/port to initialize Loop topology.
+ */
+static void
+bfa_fcs_lport_loop_init(struct bfa_fcs_lport_s *port)
+{
+}
+
+/*
+ * Called by fcs/port to notify transition to online state.
+ */
+static void
+bfa_fcs_lport_loop_online(struct bfa_fcs_lport_s *port)
+{
+        u8 num_alpa = 0, alpabm_valid = 0;
+        struct bfa_fcs_rport_s *rport;
+        u8 *alpa_map = NULL;
+        int i = 0;
+        u32 pid;
+
+        bfa_fcport_get_loop_attr(port);
+
+        num_alpa = port->port_topo.ploop.num_alpa;
+        alpabm_valid = port->port_topo.ploop.alpabm_valid;
+        alpa_map = port->port_topo.ploop.alpa_pos_map;
+
+        bfa_trc(port->fcs->bfa, port->pid);
+        bfa_trc(port->fcs->bfa, num_alpa);
+        if (alpabm_valid == 1) {
+                for (i = 0; i < num_alpa; i++) {
+                        bfa_trc(port->fcs->bfa, alpa_map[i]);
+                        if (alpa_map[i] != bfa_hton3b(port->pid)) {
+                                pid = alpa_map[i];
+                                bfa_trc(port->fcs->bfa, pid);
+                                rport = bfa_fcs_lport_get_rport_by_pid(port,
+                                                bfa_hton3b(pid));
+                                if (!rport)
+                                        rport = bfa_fcs_rport_create(port,
+                                                bfa_hton3b(pid));
+                        }
+                }
+        } else {
+                for (i = 0; i < MAX_ALPA_COUNT; i++) {
+                        if (alpa_map[i] != port->pid) {
+                                pid = loop_alpa_map[i];
+                                bfa_trc(port->fcs->bfa, pid);
+                                rport = bfa_fcs_lport_get_rport_by_pid(port,
+                                                bfa_hton3b(pid));
+                                if (!rport)
+                                        rport = bfa_fcs_rport_create(port,
+                                                bfa_hton3b(pid));
+                        }
+                }
+        }
+}
+
+/*
+ * Called by fcs/port to notify transition to offline state.
+ */
+static void
+bfa_fcs_lport_loop_offline(struct bfa_fcs_lport_s *port)
+{
+}
+
 #define BFA_FCS_FDMI_CMD_MAX_RETRIES 2
 
 /*
@@ -1888,13 +2014,10 @@ bfa_fcs_lport_fdmi_build_rhba_pyld(struct bfa_fcs_lport_fdmi_s *fdmi, u8 *pyld)
                                          sizeof(templen));
         }
 
-        /*
-         * f/w Version = driver version
-         */
         attr = (struct fdmi_attr_s *) curr_ptr;
         attr->type = cpu_to_be16(FDMI_HBA_ATTRIB_FW_VERSION);
-        templen = (u16) strlen(fcs_hba_attr->driver_version);
-        memcpy(attr->value, fcs_hba_attr->driver_version, templen);
+        templen = (u16) strlen(fcs_hba_attr->fw_version);
+        memcpy(attr->value, fcs_hba_attr->fw_version, templen);
         templen = fc_roundup(templen, sizeof(u32));
         curr_ptr += sizeof(attr->type) + sizeof(templen) + templen;
         len += templen;
@@ -2296,6 +2419,7 @@ bfa_fcs_fdmi_get_hbaattr(struct bfa_fcs_lport_fdmi_s *fdmi,
 {
         struct bfa_fcs_lport_s *port = fdmi->ms->port;
         struct bfa_fcs_driver_info_s  *driver_info = &port->fcs->driver_info;
+        struct bfa_fcs_fdmi_port_attr_s fcs_port_attr;
 
         memset(hba_attr, 0, sizeof(struct bfa_fcs_fdmi_hba_attr_s));
 
@@ -2331,7 +2455,9 @@ bfa_fcs_fdmi_get_hbaattr(struct bfa_fcs_lport_fdmi_s *fdmi,
                                 sizeof(driver_info->host_os_patch));
         }
 
-        hba_attr->max_ct_pyld = cpu_to_be32(FC_MAX_PDUSZ);
+        /* Retrieve the max frame size from the port attr */
+        bfa_fcs_fdmi_get_portattr(fdmi, &fcs_port_attr);
+        hba_attr->max_ct_pyld = fcs_port_attr.max_frm_size;
 }
 
 static void
@@ -2391,7 +2517,7 @@ bfa_fcs_fdmi_get_portattr(struct bfa_fcs_lport_fdmi_s *fdmi,
         /*
          * Max PDU Size.
          */
-        port_attr->max_frm_size = cpu_to_be32(FC_MAX_PDUSZ);
+        port_attr->max_frm_size = cpu_to_be32(pport_attr.pport_cfg.maxfrsize);
 
         /*
          * OS device Name
@@ -5199,7 +5325,7 @@ bfa_fcs_lport_scn_offline(struct bfa_fcs_lport_s *port)
 }
 
 void
-bfa_fcs_lport_scn_online(struct bfa_fcs_lport_s *port)
+bfa_fcs_lport_fab_scn_online(struct bfa_fcs_lport_s *port)
 {
         struct bfa_fcs_lport_scn_s *scn = BFA_FCS_GET_SCN_FROM_PORT(port);
 
@@ -5621,6 +5747,15 @@ bfa_fcs_lport_clear_stats(struct bfa_fcs_lport_s *fcs_port)
 }
 
 /*
+ * Let new loop map create missing rports
+ */
+void
+bfa_fcs_lport_lip_scn_online(struct bfa_fcs_lport_s *port)
+{
+        bfa_fcs_lport_loop_online(port);
+}
+
+/*
  * FCS virtual port state machine
  */
 
diff --git a/drivers/scsi/bfa/bfa_fcs_rport.c b/drivers/scsi/bfa/bfa_fcs_rport.c
index cc43b2a58ce3..58ac643ba9f3 100644
--- a/drivers/scsi/bfa/bfa_fcs_rport.c
+++ b/drivers/scsi/bfa/bfa_fcs_rport.c
@@ -106,9 +106,13 @@ static void	bfa_fcs_rport_sm_nsquery_sending(struct bfa_fcs_rport_s *rport,
                                                  enum rport_event event);
 static void     bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport,
                                          enum rport_event event);
-static void     bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
-                                                enum rport_event event);
-static void     bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport,
+static void     bfa_fcs_rport_sm_adisc_online_sending(
+                        struct bfa_fcs_rport_s *rport, enum rport_event event);
+static void     bfa_fcs_rport_sm_adisc_online(struct bfa_fcs_rport_s *rport,
+                                        enum rport_event event);
+static void     bfa_fcs_rport_sm_adisc_offline_sending(struct bfa_fcs_rport_s
+                                        *rport, enum rport_event event);
+static void     bfa_fcs_rport_sm_adisc_offline(struct bfa_fcs_rport_s *rport,
                                         enum rport_event event);
 static void     bfa_fcs_rport_sm_fc4_logorcv(struct bfa_fcs_rport_s *rport,
                                                 enum rport_event event);
@@ -150,8 +154,10 @@ static struct bfa_sm_table_s rport_sm_table[] = {
         {BFA_SM(bfa_fcs_rport_sm_online), BFA_RPORT_ONLINE},
         {BFA_SM(bfa_fcs_rport_sm_nsquery_sending), BFA_RPORT_NSQUERY},
         {BFA_SM(bfa_fcs_rport_sm_nsquery), BFA_RPORT_NSQUERY},
-        {BFA_SM(bfa_fcs_rport_sm_adisc_sending), BFA_RPORT_ADISC},
-        {BFA_SM(bfa_fcs_rport_sm_adisc), BFA_RPORT_ADISC},
+        {BFA_SM(bfa_fcs_rport_sm_adisc_online_sending), BFA_RPORT_ADISC},
+        {BFA_SM(bfa_fcs_rport_sm_adisc_online), BFA_RPORT_ADISC},
+        {BFA_SM(bfa_fcs_rport_sm_adisc_offline_sending), BFA_RPORT_ADISC},
+        {BFA_SM(bfa_fcs_rport_sm_adisc_offline), BFA_RPORT_ADISC},
         {BFA_SM(bfa_fcs_rport_sm_fc4_logorcv), BFA_RPORT_LOGORCV},
         {BFA_SM(bfa_fcs_rport_sm_fc4_logosend), BFA_RPORT_LOGO},
         {BFA_SM(bfa_fcs_rport_sm_fc4_offline), BFA_RPORT_OFFLINE},
@@ -231,10 +237,19 @@ bfa_fcs_rport_sm_plogi_sending(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_send_plogiacc(rport, NULL);
                 break;
 
+        case RPSM_EVENT_SCN_OFFLINE:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                                bfa_fcs_rport_timeout, rport,
+                                bfa_fcs_rport_del_timeout);
+                break;
         case RPSM_EVENT_ADDRESS_CHANGE:
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 /* query the NS */
                 bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+                WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+                                        BFA_PORT_TOPOLOGY_LOOP));
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
                 rport->ns_retries = 0;
                 bfa_fcs_rport_send_nsdisc(rport, NULL);
@@ -280,12 +295,20 @@ bfa_fcs_rport_sm_plogiacc_sending(struct bfa_fcs_rport_s *rport,
 
         case RPSM_EVENT_PLOGI_RCVD:
         case RPSM_EVENT_PLOGI_COMP:
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 /*
                  * Ignore, SCN is possibly online notification.
                  */
                 break;
 
+        case RPSM_EVENT_SCN_OFFLINE:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                                bfa_fcs_rport_timeout, rport,
+                                bfa_fcs_rport_del_timeout);
+                break;
+
         case RPSM_EVENT_ADDRESS_CHANGE:
                 bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
@@ -346,9 +369,19 @@ bfa_fcs_rport_sm_plogi_retry(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_send_plogiacc(rport, NULL);
                 break;
 
+        case RPSM_EVENT_SCN_OFFLINE:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_timer_stop(&rport->timer);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                                bfa_fcs_rport_timeout, rport,
+                                bfa_fcs_rport_del_timeout);
+                break;
+
         case RPSM_EVENT_ADDRESS_CHANGE:
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 bfa_timer_stop(&rport->timer);
+                WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+                                        BFA_PORT_TOPOLOGY_LOOP));
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
                 rport->ns_retries = 0;
                 bfa_fcs_rport_send_nsdisc(rport, NULL);
@@ -422,7 +455,18 @@ bfa_fcs_rport_sm_plogi(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 }
                 break;
 
-        case    RPSM_EVENT_PLOGI_RETRY:
+        case RPSM_EVENT_SCN_ONLINE:
+                break;
+
+        case RPSM_EVENT_SCN_OFFLINE:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_fcxp_discard(rport->fcxp);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                                bfa_fcs_rport_timeout, rport,
+                                bfa_fcs_rport_del_timeout);
+                break;
+
+        case RPSM_EVENT_PLOGI_RETRY:
                 rport->plogi_retries = 0;
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_retry);
                 bfa_timer_start(rport->fcs->bfa, &rport->timer,
@@ -440,8 +484,10 @@ bfa_fcs_rport_sm_plogi(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 break;
 
         case RPSM_EVENT_ADDRESS_CHANGE:
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 bfa_fcxp_discard(rport->fcxp);
+                WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+                                        BFA_PORT_TOPOLOGY_LOOP));
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
                 rport->ns_retries = 0;
                 bfa_fcs_rport_send_nsdisc(rport, NULL);
@@ -512,7 +558,8 @@ bfa_fcs_rport_sm_fc4_fcs_online(struct bfa_fcs_rport_s *rport,
         case RPSM_EVENT_PLOGI_COMP:
         case RPSM_EVENT_LOGO_IMP:
         case RPSM_EVENT_ADDRESS_CHANGE:
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
+        case RPSM_EVENT_SCN_OFFLINE:
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_offline);
                 bfa_fcs_rport_fcs_offline_action(rport);
                 break;
@@ -561,9 +608,10 @@ bfa_fcs_rport_sm_hal_online(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_fcs_offline_action(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_LOGO_IMP:
         case RPSM_EVENT_ADDRESS_CHANGE:
+        case RPSM_EVENT_SCN_OFFLINE:
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_offline);
                 bfa_fcs_rport_fcs_offline_action(rport);
                 break;
@@ -595,14 +643,15 @@ bfa_fcs_rport_sm_online(struct bfa_fcs_rport_s *rport, enum rport_event event)
         bfa_trc(rport->fcs, event);
 
         switch (event) {
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 if (bfa_fcs_fabric_is_switched(rport->port->fabric)) {
                         bfa_sm_set_state(rport,
                                          bfa_fcs_rport_sm_nsquery_sending);
                         rport->ns_retries = 0;
                         bfa_fcs_rport_send_nsdisc(rport, NULL);
                 } else {
-                        bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_sending);
+                        bfa_sm_set_state(rport,
+                                bfa_fcs_rport_sm_adisc_online_sending);
                         bfa_fcs_rport_send_adisc(rport, NULL);
                 }
                 break;
@@ -610,6 +659,7 @@ bfa_fcs_rport_sm_online(struct bfa_fcs_rport_s *rport, enum rport_event event)
         case RPSM_EVENT_PLOGI_RCVD:
         case RPSM_EVENT_LOGO_IMP:
         case RPSM_EVENT_ADDRESS_CHANGE:
+        case RPSM_EVENT_SCN_OFFLINE:
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_offline);
                 bfa_fcs_rport_hal_offline_action(rport);
                 break;
@@ -625,6 +675,7 @@ bfa_fcs_rport_sm_online(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 bfa_fcs_rport_hal_offline_action(rport);
                 break;
 
+        case RPSM_EVENT_SCN_ONLINE:
         case RPSM_EVENT_PLOGI_COMP:
                 break;
 
@@ -656,7 +707,7 @@ bfa_fcs_rport_sm_nsquery_sending(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_hal_offline_action(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 /*
                  * ignore SCN, wait for response to query itself
                  */
@@ -696,7 +747,7 @@ bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport, enum rport_event event)
 
         switch (event) {
         case RPSM_EVENT_ACCEPTED:
-                bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_sending);
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_online_sending);
                 bfa_fcs_rport_send_adisc(rport, NULL);
                 break;
 
@@ -718,7 +769,7 @@ bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 bfa_fcs_rport_hal_offline_action(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 break;
 
         case RPSM_EVENT_LOGO_RCVD:
@@ -747,7 +798,7 @@ bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport, enum rport_event event)
  *      authenticating with rport. FC-4s are paused.
  */
 static void
-bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
+bfa_fcs_rport_sm_adisc_online_sending(struct bfa_fcs_rport_s *rport,
          enum rport_event event)
 {
         bfa_trc(rport->fcs, rport->pwwn);
@@ -756,7 +807,7 @@ bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
 
         switch (event) {
         case RPSM_EVENT_FCXP_SENT:
-                bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc);
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_online);
                 break;
 
         case RPSM_EVENT_DELETE:
@@ -779,7 +830,7 @@ bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_hal_offline_action(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 break;
 
         case RPSM_EVENT_PLOGI_RCVD:
@@ -798,7 +849,8 @@ bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
  *              FC-4s are paused.
  */
 static void
-bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport, enum rport_event event)
+bfa_fcs_rport_sm_adisc_online(struct bfa_fcs_rport_s *rport,
+                                enum rport_event event)
 {
         bfa_trc(rport->fcs, rport->pwwn);
         bfa_trc(rport->fcs, rport->pid);
@@ -831,7 +883,7 @@ bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 bfa_fcs_rport_hal_offline_action(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 /*
                  * already processing RSCN
                  */
@@ -856,7 +908,96 @@ bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport, enum rport_event event)
 }
 
 /*
- *              Rport has sent LOGO. Awaiting FC-4 offline completion callback.
+ * ADISC is being sent for authenticating with rport
+ * Already did offline actions.
+ */
+static void
+bfa_fcs_rport_sm_adisc_offline_sending(struct bfa_fcs_rport_s *rport,
+        enum rport_event event)
+{
+        bfa_trc(rport->fcs, rport->pwwn);
+        bfa_trc(rport->fcs, rport->pid);
+        bfa_trc(rport->fcs, event);
+
+        switch (event) {
+        case RPSM_EVENT_FCXP_SENT:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_offline);
+                break;
+
+        case RPSM_EVENT_DELETE:
+        case RPSM_EVENT_SCN_OFFLINE:
+        case RPSM_EVENT_LOGO_IMP:
+        case RPSM_EVENT_LOGO_RCVD:
+        case RPSM_EVENT_PRLO_RCVD:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_fcxp_walloc_cancel(rport->fcs->bfa,
+                        &rport->fcxp_wqe);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                        bfa_fcs_rport_timeout, rport,
+                        bfa_fcs_rport_del_timeout);
+                break;
+
+        case RPSM_EVENT_PLOGI_RCVD:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogiacc_sending);
+                bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+                bfa_fcs_rport_send_plogiacc(rport, NULL);
+                break;
+
+        default:
+                bfa_sm_fault(rport->fcs, event);
+        }
+}
+
+/*
+ * ADISC to rport
+ * Already did offline actions
+ */
+static void
+bfa_fcs_rport_sm_adisc_offline(struct bfa_fcs_rport_s *rport,
+                        enum rport_event event)
+{
+        bfa_trc(rport->fcs, rport->pwwn);
+        bfa_trc(rport->fcs, rport->pid);
+        bfa_trc(rport->fcs, event);
+
+        switch (event) {
+        case RPSM_EVENT_ACCEPTED:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_hal_online);
+                bfa_fcs_rport_hal_online(rport);
+                break;
+
+        case RPSM_EVENT_PLOGI_RCVD:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogiacc_sending);
+                bfa_fcxp_discard(rport->fcxp);
+                bfa_fcs_rport_send_plogiacc(rport, NULL);
+                break;
+
+        case RPSM_EVENT_FAILED:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                        bfa_fcs_rport_timeout, rport,
+                        bfa_fcs_rport_del_timeout);
+                break;
+
+        case RPSM_EVENT_DELETE:
+        case RPSM_EVENT_SCN_OFFLINE:
+        case RPSM_EVENT_LOGO_IMP:
+        case RPSM_EVENT_LOGO_RCVD:
+        case RPSM_EVENT_PRLO_RCVD:
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+                bfa_fcxp_discard(rport->fcxp);
+                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                        bfa_fcs_rport_timeout, rport,
+                        bfa_fcs_rport_del_timeout);
+                break;
+
+        default:
+                bfa_sm_fault(rport->fcs, event);
+        }
+}
+
+/*
+ * Rport has sent LOGO. Awaiting FC-4 offline completion callback.
  */
 static void
 bfa_fcs_rport_sm_fc4_logorcv(struct bfa_fcs_rport_s *rport,
@@ -881,6 +1022,8 @@ bfa_fcs_rport_sm_fc4_logorcv(struct bfa_fcs_rport_s *rport,
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_off_delete);
                 break;
 
+        case RPSM_EVENT_SCN_ONLINE:
+        case RPSM_EVENT_SCN_OFFLINE:
         case RPSM_EVENT_HCB_ONLINE:
         case RPSM_EVENT_LOGO_RCVD:
         case RPSM_EVENT_PRLO_RCVD:
@@ -945,6 +1088,8 @@ bfa_fcs_rport_sm_fc4_offline(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_hal_offline(rport);
                 break;
 
+        case RPSM_EVENT_SCN_ONLINE:
+                break;
         case RPSM_EVENT_LOGO_RCVD:
                 /*
                  * Rport is going offline. Just ack the logo
@@ -956,8 +1101,9 @@ bfa_fcs_rport_sm_fc4_offline(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_send_prlo_acc(rport);
                 break;
 
+        case RPSM_EVENT_SCN_OFFLINE:
         case RPSM_EVENT_HCB_ONLINE:
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_LOGO_IMP:
         case RPSM_EVENT_ADDRESS_CHANGE:
                 /*
@@ -1015,6 +1161,19 @@ bfa_fcs_rport_sm_hcb_offline(struct bfa_fcs_rport_s *rport,
                                 bfa_fcs_rport_sm_nsdisc_sending);
                         rport->ns_retries = 0;
                         bfa_fcs_rport_send_nsdisc(rport, NULL);
+                } else if (bfa_fcport_get_topology(rport->port->fcs->bfa) ==
+                                        BFA_PORT_TOPOLOGY_LOOP) {
+                        if (rport->scn_online) {
+                                bfa_sm_set_state(rport,
+                                        bfa_fcs_rport_sm_adisc_offline_sending);
+                                bfa_fcs_rport_send_adisc(rport, NULL);
+                        } else {
+                                bfa_sm_set_state(rport,
+                                        bfa_fcs_rport_sm_offline);
+                                bfa_timer_start(rport->fcs->bfa, &rport->timer,
+                                        bfa_fcs_rport_timeout, rport,
+                                        bfa_fcs_rport_del_timeout);
+                        }
                 } else {
                         bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_sending);
                         rport->plogi_retries = 0;
@@ -1027,7 +1186,9 @@ bfa_fcs_rport_sm_hcb_offline(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_free(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_SCN_ONLINE:
+        case RPSM_EVENT_SCN_OFFLINE:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_LOGO_RCVD:
         case RPSM_EVENT_PRLO_RCVD:
         case RPSM_EVENT_PLOGI_RCVD:
@@ -1106,6 +1267,8 @@ bfa_fcs_rport_sm_hcb_logorcv(struct bfa_fcs_rport_s *rport,
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_hcb_offline);
                 break;
 
+        case RPSM_EVENT_SCN_ONLINE:
+        case RPSM_EVENT_SCN_OFFLINE:
         case RPSM_EVENT_LOGO_RCVD:
         case RPSM_EVENT_PRLO_RCVD:
                 /*
@@ -1146,6 +1309,8 @@ bfa_fcs_rport_sm_hcb_logosend(struct bfa_fcs_rport_s *rport,
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_delete_pending);
                 break;
 
+        case RPSM_EVENT_SCN_ONLINE:
+        case RPSM_EVENT_SCN_OFFLINE:
         case RPSM_EVENT_ADDRESS_CHANGE:
                 break;
 
@@ -1172,7 +1337,9 @@ bfa_fcs_rport_sm_logo_sending(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_free(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_SCN_ONLINE:
+        case RPSM_EVENT_SCN_OFFLINE:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_ADDRESS_CHANGE:
                 break;
 
@@ -1209,10 +1376,12 @@ bfa_fcs_rport_sm_offline(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 bfa_fcs_rport_free(rport);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_ADDRESS_CHANGE:
-                bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
                 bfa_timer_stop(&rport->timer);
+                WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+                                        BFA_PORT_TOPOLOGY_LOOP));
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
                 rport->ns_retries = 0;
                 bfa_fcs_rport_send_nsdisc(rport, NULL);
                 break;
@@ -1232,6 +1401,7 @@ bfa_fcs_rport_sm_offline(struct bfa_fcs_rport_s *rport, enum rport_event event)
         case RPSM_EVENT_LOGO_RCVD:
         case RPSM_EVENT_PRLO_RCVD:
         case RPSM_EVENT_LOGO_IMP:
+        case RPSM_EVENT_SCN_OFFLINE:
                 break;
 
         case RPSM_EVENT_PLOGI_COMP:
@@ -1240,6 +1410,12 @@ bfa_fcs_rport_sm_offline(struct bfa_fcs_rport_s *rport, enum rport_event event)
                 bfa_fcs_rport_fcs_online_action(rport);
                 break;
 
+        case RPSM_EVENT_SCN_ONLINE:
+                bfa_timer_stop(&rport->timer);
+                bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_sending);
+                bfa_fcs_rport_send_plogi(rport, NULL);
+                break;
+
         case RPSM_EVENT_PLOGI_SEND:
                 bfa_timer_stop(&rport->timer);
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_sending);
@@ -1280,7 +1456,7 @@ bfa_fcs_rport_sm_nsdisc_sending(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_send_plogiacc(rport, NULL);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_LOGO_RCVD:
         case RPSM_EVENT_PRLO_RCVD:
         case RPSM_EVENT_PLOGI_SEND:
@@ -1326,7 +1502,7 @@ bfa_fcs_rport_sm_nsdisc_retry(struct bfa_fcs_rport_s *rport,
                 bfa_fcs_rport_send_nsdisc(rport, NULL);
                 break;
 
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
         case RPSM_EVENT_ADDRESS_CHANGE:
                 bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
                 bfa_timer_stop(&rport->timer);
@@ -1439,7 +1615,7 @@ bfa_fcs_rport_sm_nsdisc_sent(struct bfa_fcs_rport_s *rport,
         case RPSM_EVENT_PRLO_RCVD:
                 bfa_fcs_rport_send_prlo_acc(rport);
                 break;
-        case RPSM_EVENT_SCN:
+        case RPSM_EVENT_FAB_SCN:
                 /*
                  * ignore, wait for NS query response
                  */
@@ -2546,7 +2722,7 @@ void
 bfa_fcs_rport_scn(struct bfa_fcs_rport_s *rport)
 {
         rport->stats.rscns++;
-        bfa_sm_send_event(rport, RPSM_EVENT_SCN);
+        bfa_sm_send_event(rport, RPSM_EVENT_FAB_SCN);
 }
 
 /*
@@ -2621,6 +2797,48 @@ bfa_cb_rport_qos_scn_flowid(void *cbarg,
         bfa_fcs_rport_aen_post(rport, BFA_RPORT_AEN_QOS_FLOWID, &aen_data);
 }
 
+void
+bfa_cb_rport_scn_online(struct bfa_s *bfa)
+{
+        struct bfa_fcs_s *fcs = &((struct bfad_s *)bfa->bfad)->bfa_fcs;
+        struct bfa_fcs_lport_s *port = bfa_fcs_get_base_port(fcs);
+        struct bfa_fcs_rport_s *rp;
+        struct list_head *qe;
+
+        list_for_each(qe, &port->rport_q) {
+                rp = (struct bfa_fcs_rport_s *) qe;
+                bfa_sm_send_event(rp, RPSM_EVENT_SCN_ONLINE);
+                rp->scn_online = BFA_TRUE;
+        }
+
+        if (bfa_fcs_lport_is_online(port))
+                bfa_fcs_lport_lip_scn_online(port);
+}
+
+void
+bfa_cb_rport_scn_no_dev(void *rport)
+{
+        struct bfa_fcs_rport_s *rp = rport;
+
+        bfa_sm_send_event(rp, RPSM_EVENT_SCN_OFFLINE);
+        rp->scn_online = BFA_FALSE;
+}
+
+void
+bfa_cb_rport_scn_offline(struct bfa_s *bfa)
+{
+        struct bfa_fcs_s *fcs = &((struct bfad_s *)bfa->bfad)->bfa_fcs;
+        struct bfa_fcs_lport_s *port = bfa_fcs_get_base_port(fcs);
+        struct bfa_fcs_rport_s *rp;
+        struct list_head *qe;
+
+        list_for_each(qe, &port->rport_q) {
+                rp = (struct bfa_fcs_rport_s *) qe;
+                bfa_sm_send_event(rp, RPSM_EVENT_SCN_OFFLINE);
+                rp->scn_online = BFA_FALSE;
+        }
+}
+
 /*
  *      brief
  *      This routine is a static BFA callback when there is a QoS priority
@@ -2808,6 +3026,9 @@ bfa_fcs_rport_get_attr(struct bfa_fcs_rport_s *rport,
         struct bfa_rport_qos_attr_s qos_attr;
         struct bfa_fcs_lport_s *port = rport->port;
         bfa_port_speed_t rport_speed = rport->rpf.rpsc_speed;
+        struct bfa_port_attr_s port_attr;
+
+        bfa_fcport_get_attr(rport->fcs->bfa, &port_attr);
 
         memset(rport_attr, 0, sizeof(struct bfa_rport_attr_s));
         memset(&qos_attr, 0, sizeof(struct bfa_rport_qos_attr_s));
@@ -2838,7 +3059,8 @@ bfa_fcs_rport_get_attr(struct bfa_fcs_rport_s *rport,
                         rport_speed =
                                 bfa_fcport_get_ratelim_speed(rport->fcs->bfa);
 
-                if (rport_speed < bfa_fcs_lport_get_rport_max_speed(port))
+                if ((bfa_fcs_lport_get_rport_max_speed(port) !=
+                    BFA_PORT_SPEED_UNKNOWN) && (rport_speed < port_attr.speed))
                         rport_attr->trl_enforced = BFA_TRUE;
         }
 }
diff --git a/drivers/scsi/bfa/bfa_ioc.c b/drivers/scsi/bfa/bfa_ioc.c
index 75ca8752b9f4..0116c1032e25 100644
--- a/drivers/scsi/bfa/bfa_ioc.c
+++ b/drivers/scsi/bfa/bfa_ioc.c
@@ -731,8 +731,7 @@ bfa_iocpf_sm_fwcheck_entry(struct bfa_iocpf_s *iocpf)
         /*
          * Unlock the hw semaphore. Should be here only once per boot.
          */
-        readl(iocpf->ioc->ioc_regs.ioc_sem_reg);
-        writel(1, iocpf->ioc->ioc_regs.ioc_sem_reg);
+        bfa_ioc_ownership_reset(iocpf->ioc);
 
         /*
          * unlock init semaphore.
@@ -1751,6 +1750,7 @@ bfa_ioc_getattr_reply(struct bfa_ioc_s *ioc)
         attr->card_type     = be32_to_cpu(attr->card_type);
         attr->maxfrsize     = be16_to_cpu(attr->maxfrsize);
         ioc->fcmode     = (attr->port_mode == BFI_PORT_MODE_FC);
+        attr->mfg_year  = be16_to_cpu(attr->mfg_year);
 
         bfa_fsm_send_event(ioc, IOC_E_FWRSP_GETATTR);
 }
@@ -2497,6 +2497,9 @@ bfa_ioc_get_adapter_attr(struct bfa_ioc_s *ioc,
         ad_attr->cna_capable = bfa_ioc_is_cna(ioc);
         ad_attr->trunk_capable = (ad_attr->nports > 1) &&
                                   !bfa_ioc_is_cna(ioc) && !ad_attr->is_mezz;
+        ad_attr->mfg_day = ioc_attr->mfg_day;
+        ad_attr->mfg_month = ioc_attr->mfg_month;
+        ad_attr->mfg_year = ioc_attr->mfg_year;
 }
 
 enum bfa_ioc_type_e
@@ -2923,7 +2926,7 @@ bfa_ioc_poll_fwinit(struct bfa_ioc_s *ioc)
                 return;
         }
 
-        if (ioc->iocpf.poll_time >= BFA_IOC_TOV)
+        if (ioc->iocpf.poll_time >= (3 * BFA_IOC_TOV))
                 bfa_iocpf_timeout(ioc);
         else {
                 ioc->iocpf.poll_time += BFA_IOC_POLL_TOV;
@@ -3016,7 +3019,6 @@ bfa_ablk_config_swap(struct bfa_ablk_cfg_s *cfg)
         struct bfa_ablk_cfg_inst_s *cfg_inst;
         int i, j;
         u16     be16;
-        u32     be32;
 
         for (i = 0; i < BFA_ABLK_MAX; i++) {
                 cfg_inst = &cfg->inst[i];
@@ -3027,8 +3029,10 @@ bfa_ablk_config_swap(struct bfa_ablk_cfg_s *cfg)
                         cfg_inst->pf_cfg[j].num_qpairs = be16_to_cpu(be16);
                         be16 = cfg_inst->pf_cfg[j].num_vectors;
                         cfg_inst->pf_cfg[j].num_vectors = be16_to_cpu(be16);
-                        be32 = cfg_inst->pf_cfg[j].bw;
-                        cfg_inst->pf_cfg[j].bw = be16_to_cpu(be32);
+                        be16 = cfg_inst->pf_cfg[j].bw_min;
+                        cfg_inst->pf_cfg[j].bw_min = be16_to_cpu(be16);
+                        be16 = cfg_inst->pf_cfg[j].bw_max;
+                        cfg_inst->pf_cfg[j].bw_max = be16_to_cpu(be16);
                 }
         }
 }
@@ -3170,7 +3174,8 @@ bfa_ablk_query(struct bfa_ablk_s *ablk, struct bfa_ablk_cfg_s *ablk_cfg,
 
 bfa_status_t
 bfa_ablk_pf_create(struct bfa_ablk_s *ablk, u16 *pcifn,
-                u8 port, enum bfi_pcifn_class personality, int bw,
+                u8 port, enum bfi_pcifn_class personality,
+                u16 bw_min, u16 bw_max,
                 bfa_ablk_cbfn_t cbfn, void *cbarg)
 {
         struct bfi_ablk_h2i_pf_req_s *m;
@@ -3194,7 +3199,8 @@ bfa_ablk_pf_create(struct bfa_ablk_s *ablk, u16 *pcifn,
         bfi_h2i_set(m->mh, BFI_MC_ABLK, BFI_ABLK_H2I_PF_CREATE,
                     bfa_ioc_portid(ablk->ioc));
         m->pers = cpu_to_be16((u16)personality);
-        m->bw = cpu_to_be32(bw);
+        m->bw_min = cpu_to_be16(bw_min);
+        m->bw_max = cpu_to_be16(bw_max);
         m->port = port;
         bfa_ioc_mbox_queue(ablk->ioc, &ablk->mb);
 
@@ -3294,8 +3300,8 @@ bfa_ablk_port_config(struct bfa_ablk_s *ablk, int port, enum bfa_mode_s mode,
 }
 
 bfa_status_t
-bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, int bw,
-                bfa_ablk_cbfn_t cbfn, void *cbarg)
+bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, u16 bw_min,
+                   u16 bw_max, bfa_ablk_cbfn_t cbfn, void *cbarg)
 {
         struct bfi_ablk_h2i_pf_req_s *m;
 
@@ -3317,7 +3323,8 @@ bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, int bw,
         bfi_h2i_set(m->mh, BFI_MC_ABLK, BFI_ABLK_H2I_PF_UPDATE,
                 bfa_ioc_portid(ablk->ioc));
         m->pcifn = (u8)pcifn;
-        m->bw = cpu_to_be32(bw);
+        m->bw_min = cpu_to_be16(bw_min);
+        m->bw_max = cpu_to_be16(bw_max);
         bfa_ioc_mbox_queue(ablk->ioc, &ablk->mb);
 
         return BFA_STATUS_OK;
@@ -4680,22 +4687,25 @@ diag_tempsensor_comp(struct bfa_diag_s *diag, bfi_diag_ts_rsp_t *rsp)
         diag->tsensor.temp->temp = be16_to_cpu(rsp->temp);
         diag->tsensor.temp->ts_junc = rsp->ts_junc;
         diag->tsensor.temp->ts_brd = rsp->ts_brd;
-        diag->tsensor.temp->status = BFA_STATUS_OK;
 
         if (rsp->ts_brd) {
+                /* tsensor.temp->status is brd_temp status */
+                diag->tsensor.temp->status = rsp->status;
                 if (rsp->status == BFA_STATUS_OK) {
                         diag->tsensor.temp->brd_temp =
                                 be16_to_cpu(rsp->brd_temp);
-                } else {
-                        bfa_trc(diag, rsp->status);
+                } else
                         diag->tsensor.temp->brd_temp = 0;
-                        diag->tsensor.temp->status = BFA_STATUS_DEVBUSY;
-                }
         }
+
+        bfa_trc(diag, rsp->status);
         bfa_trc(diag, rsp->ts_junc);
         bfa_trc(diag, rsp->temp);
         bfa_trc(diag, rsp->ts_brd);
         bfa_trc(diag, rsp->brd_temp);
+
+        /* tsensor status is always good bcos we always have junction temp */
+        diag->tsensor.status = BFA_STATUS_OK;
         diag->tsensor.cbfn(diag->tsensor.cbarg, diag->tsensor.status);
         diag->tsensor.lock = 0;
 }
@@ -4924,6 +4934,7 @@ bfa_diag_tsensor_query(struct bfa_diag_s *diag,
         diag->tsensor.temp = result;
         diag->tsensor.cbfn = cbfn;
         diag->tsensor.cbarg = cbarg;
+        diag->tsensor.status = BFA_STATUS_OK;
 
         /* Send msg to fw */
         diag_tempsensor_send(diag);
@@ -5615,7 +5626,7 @@ bfa_dconf_sm_uninit(struct bfa_dconf_mod_s *dconf, enum bfa_dconf_event event)
                 }
                 bfa_sm_set_state(dconf, bfa_dconf_sm_flash_read);
                 bfa_timer_start(dconf->bfa, &dconf->timer,
-                        bfa_dconf_timer, dconf, BFA_DCONF_UPDATE_TOV);
+                        bfa_dconf_timer, dconf, 2 * BFA_DCONF_UPDATE_TOV);
                 bfa_status = bfa_flash_read_part(BFA_FLASH(dconf->bfa),
                                         BFA_FLASH_PART_DRV, dconf->instance,
                                         dconf->dconf,
@@ -5655,7 +5666,7 @@ bfa_dconf_sm_flash_read(struct bfa_dconf_mod_s *dconf,
                 break;
         case BFA_DCONF_SM_TIMEOUT:
                 bfa_sm_set_state(dconf, bfa_dconf_sm_ready);
-                bfa_fsm_send_event(&dconf->bfa->iocfc, IOCFC_E_IOC_FAILED);
+                bfa_ioc_suspend(&dconf->bfa->ioc);
                 break;
         case BFA_DCONF_SM_EXIT:
                 bfa_timer_stop(&dconf->timer);
@@ -5853,7 +5864,6 @@ bfa_dconf_init_cb(void *arg, bfa_status_t status)
         struct bfa_s *bfa = arg;
         struct bfa_dconf_mod_s *dconf = BFA_DCONF_MOD(bfa);
 
-        bfa_sm_send_event(dconf, BFA_DCONF_SM_FLASH_COMP);
         if (status == BFA_STATUS_OK) {
                 bfa_dconf_read_data_valid(bfa) = BFA_TRUE;
                 if (dconf->dconf->hdr.signature != BFI_DCONF_SIGNATURE)
@@ -5861,6 +5871,7 @@ bfa_dconf_init_cb(void *arg, bfa_status_t status)
                 if (dconf->dconf->hdr.version != BFI_DCONF_VERSION)
                         dconf->dconf->hdr.version = BFI_DCONF_VERSION;
         }
+        bfa_sm_send_event(dconf, BFA_DCONF_SM_FLASH_COMP);
         bfa_fsm_send_event(&bfa->iocfc, IOCFC_E_DCONF_DONE);
 }
 
@@ -5945,3 +5956,448 @@ bfa_dconf_modexit(struct bfa_s *bfa)
         struct bfa_dconf_mod_s *dconf = BFA_DCONF_MOD(bfa);
         bfa_sm_send_event(dconf, BFA_DCONF_SM_EXIT);
 }
+
+/*
+ * FRU specific functions
+ */
+
+#define BFA_FRU_DMA_BUF_SZ      0x02000         /* 8k dma buffer */
+#define BFA_FRU_CHINOOK_MAX_SIZE 0x10000
+#define BFA_FRU_LIGHTNING_MAX_SIZE 0x200
+
+static void
+bfa_fru_notify(void *cbarg, enum bfa_ioc_event_e event)
+{
+        struct bfa_fru_s *fru = cbarg;
+
+        bfa_trc(fru, event);
+
+        switch (event) {
+        case BFA_IOC_E_DISABLED:
+        case BFA_IOC_E_FAILED:
+                if (fru->op_busy) {
+                        fru->status = BFA_STATUS_IOC_FAILURE;
+                        fru->cbfn(fru->cbarg, fru->status);
+                        fru->op_busy = 0;
+                }
+                break;
+
+        default:
+                break;
+        }
+}
+
+/*
+ * Send fru write request.
+ *
+ * @param[in] cbarg - callback argument
+ */
+static void
+bfa_fru_write_send(void *cbarg, enum bfi_fru_h2i_msgs msg_type)
+{
+        struct bfa_fru_s *fru = cbarg;
+        struct bfi_fru_write_req_s *msg =
+                        (struct bfi_fru_write_req_s *) fru->mb.msg;
+        u32 len;
+
+        msg->offset = cpu_to_be32(fru->addr_off + fru->offset);
+        len = (fru->residue < BFA_FRU_DMA_BUF_SZ) ?
+                                fru->residue : BFA_FRU_DMA_BUF_SZ;
+        msg->length = cpu_to_be32(len);
+
+        /*
+         * indicate if it's the last msg of the whole write operation
+         */
+        msg->last = (len == fru->residue) ? 1 : 0;
+
+        bfi_h2i_set(msg->mh, BFI_MC_FRU, msg_type, bfa_ioc_portid(fru->ioc));
+        bfa_alen_set(&msg->alen, len, fru->dbuf_pa);
+
+        memcpy(fru->dbuf_kva, fru->ubuf + fru->offset, len);
+        bfa_ioc_mbox_queue(fru->ioc, &fru->mb);
+
+        fru->residue -= len;
+        fru->offset += len;
+}
+
+/*
+ * Send fru read request.
+ *
+ * @param[in] cbarg - callback argument
+ */
+static void
+bfa_fru_read_send(void *cbarg, enum bfi_fru_h2i_msgs msg_type)
+{
+        struct bfa_fru_s *fru = cbarg;
+        struct bfi_fru_read_req_s *msg =
+                        (struct bfi_fru_read_req_s *) fru->mb.msg;
+        u32 len;
+
+        msg->offset = cpu_to_be32(fru->addr_off + fru->offset);
+        len = (fru->residue < BFA_FRU_DMA_BUF_SZ) ?
+                                fru->residue : BFA_FRU_DMA_BUF_SZ;
+        msg->length = cpu_to_be32(len);
+        bfi_h2i_set(msg->mh, BFI_MC_FRU, msg_type, bfa_ioc_portid(fru->ioc));
+        bfa_alen_set(&msg->alen, len, fru->dbuf_pa);
+        bfa_ioc_mbox_queue(fru->ioc, &fru->mb);
+}
+
+/*
+ * Flash memory info API.
+ *
+ * @param[in] mincfg - minimal cfg variable
+ */
+u32
+bfa_fru_meminfo(bfa_boolean_t mincfg)
+{
+        /* min driver doesn't need fru */
+        if (mincfg)
+                return 0;
+
+        return BFA_ROUNDUP(BFA_FRU_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
+}
+
+/*
+ * Flash attach API.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] ioc  - ioc structure
+ * @param[in] dev  - device structure
+ * @param[in] trcmod - trace module
+ * @param[in] logmod - log module
+ */
+void
+bfa_fru_attach(struct bfa_fru_s *fru, struct bfa_ioc_s *ioc, void *dev,
+        struct bfa_trc_mod_s *trcmod, bfa_boolean_t mincfg)
+{
+        fru->ioc = ioc;
+        fru->trcmod = trcmod;
+        fru->cbfn = NULL;
+        fru->cbarg = NULL;
+        fru->op_busy = 0;
+
+        bfa_ioc_mbox_regisr(fru->ioc, BFI_MC_FRU, bfa_fru_intr, fru);
+        bfa_q_qe_init(&fru->ioc_notify);
+        bfa_ioc_notify_init(&fru->ioc_notify, bfa_fru_notify, fru);
+        list_add_tail(&fru->ioc_notify.qe, &fru->ioc->notify_q);
+
+        /* min driver doesn't need fru */
+        if (mincfg) {
+                fru->dbuf_kva = NULL;
+                fru->dbuf_pa = 0;
+        }
+}
+
+/*
+ * Claim memory for fru
+ *
+ * @param[in] fru - fru structure
+ * @param[in] dm_kva - pointer to virtual memory address
+ * @param[in] dm_pa - frusical memory address
+ * @param[in] mincfg - minimal cfg variable
+ */
+void
+bfa_fru_memclaim(struct bfa_fru_s *fru, u8 *dm_kva, u64 dm_pa,
+        bfa_boolean_t mincfg)
+{
+        if (mincfg)
+                return;
+
+        fru->dbuf_kva = dm_kva;
+        fru->dbuf_pa = dm_pa;
+        memset(fru->dbuf_kva, 0, BFA_FRU_DMA_BUF_SZ);
+        dm_kva += BFA_ROUNDUP(BFA_FRU_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
+        dm_pa += BFA_ROUNDUP(BFA_FRU_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
+}
+
+/*
+ * Update fru vpd image.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - update data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_fruvpd_update(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+                  bfa_cb_fru_t cbfn, void *cbarg)
+{
+        bfa_trc(fru, BFI_FRUVPD_H2I_WRITE_REQ);
+        bfa_trc(fru, len);
+        bfa_trc(fru, offset);
+
+        if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+                return BFA_STATUS_FRU_NOT_PRESENT;
+
+        if (fru->ioc->attr->card_type != BFA_MFG_TYPE_CHINOOK)
+                return BFA_STATUS_CMD_NOTSUPP;
+
+        if (!bfa_ioc_is_operational(fru->ioc))
+                return BFA_STATUS_IOC_NON_OP;
+
+        if (fru->op_busy) {
+                bfa_trc(fru, fru->op_busy);
+                return BFA_STATUS_DEVBUSY;
+        }
+
+        fru->op_busy = 1;
+
+        fru->cbfn = cbfn;
+        fru->cbarg = cbarg;
+        fru->residue = len;
+        fru->offset = 0;
+        fru->addr_off = offset;
+        fru->ubuf = buf;
+
+        bfa_fru_write_send(fru, BFI_FRUVPD_H2I_WRITE_REQ);
+
+        return BFA_STATUS_OK;
+}
+
+/*
+ * Read fru vpd image.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - read data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_fruvpd_read(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+                bfa_cb_fru_t cbfn, void *cbarg)
+{
+        bfa_trc(fru, BFI_FRUVPD_H2I_READ_REQ);
+        bfa_trc(fru, len);
+        bfa_trc(fru, offset);
+
+        if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+                return BFA_STATUS_FRU_NOT_PRESENT;
+
+        if (fru->ioc->attr->card_type != BFA_MFG_TYPE_CHINOOK)
+                return BFA_STATUS_CMD_NOTSUPP;
+
+        if (!bfa_ioc_is_operational(fru->ioc))
+                return BFA_STATUS_IOC_NON_OP;
+
+        if (fru->op_busy) {
+                bfa_trc(fru, fru->op_busy);
+                return BFA_STATUS_DEVBUSY;
+        }
+
+        fru->op_busy = 1;
+
+        fru->cbfn = cbfn;
+        fru->cbarg = cbarg;
+        fru->residue = len;
+        fru->offset = 0;
+        fru->addr_off = offset;
+        fru->ubuf = buf;
+        bfa_fru_read_send(fru, BFI_FRUVPD_H2I_READ_REQ);
+
+        return BFA_STATUS_OK;
+}
+
+/*
+ * Get maximum size fru vpd image.
+ *
+ * @param[in] fru - fru structure
+ * @param[out] size - maximum size of fru vpd data
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_fruvpd_get_max_size(struct bfa_fru_s *fru, u32 *max_size)
+{
+        if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+                return BFA_STATUS_FRU_NOT_PRESENT;
+
+        if (!bfa_ioc_is_operational(fru->ioc))
+                return BFA_STATUS_IOC_NON_OP;
+
+        if (fru->ioc->attr->card_type == BFA_MFG_TYPE_CHINOOK)
+                *max_size = BFA_FRU_CHINOOK_MAX_SIZE;
+        else
+                return BFA_STATUS_CMD_NOTSUPP;
+        return BFA_STATUS_OK;
+}
+/*
+ * tfru write.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - update data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_tfru_write(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+               bfa_cb_fru_t cbfn, void *cbarg)
+{
+        bfa_trc(fru, BFI_TFRU_H2I_WRITE_REQ);
+        bfa_trc(fru, len);
+        bfa_trc(fru, offset);
+        bfa_trc(fru, *((u8 *) buf));
+
+        if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+                return BFA_STATUS_FRU_NOT_PRESENT;
+
+        if (!bfa_ioc_is_operational(fru->ioc))
+                return BFA_STATUS_IOC_NON_OP;
+
+        if (fru->op_busy) {
+                bfa_trc(fru, fru->op_busy);
+                return BFA_STATUS_DEVBUSY;
+        }
+
+        fru->op_busy = 1;
+
+        fru->cbfn = cbfn;
+        fru->cbarg = cbarg;
+        fru->residue = len;
+        fru->offset = 0;
+        fru->addr_off = offset;
+        fru->ubuf = buf;
+
+        bfa_fru_write_send(fru, BFI_TFRU_H2I_WRITE_REQ);
+
+        return BFA_STATUS_OK;
+}
+
+/*
+ * tfru read.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - read data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_tfru_read(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+              bfa_cb_fru_t cbfn, void *cbarg)
+{
+        bfa_trc(fru, BFI_TFRU_H2I_READ_REQ);
+        bfa_trc(fru, len);
+        bfa_trc(fru, offset);
+
+        if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+                return BFA_STATUS_FRU_NOT_PRESENT;
+
+        if (!bfa_ioc_is_operational(fru->ioc))
+                return BFA_STATUS_IOC_NON_OP;
+
+        if (fru->op_busy) {
+                bfa_trc(fru, fru->op_busy);
+                return BFA_STATUS_DEVBUSY;
+        }
+
+        fru->op_busy = 1;
+
+        fru->cbfn = cbfn;
+        fru->cbarg = cbarg;
+        fru->residue = len;
+        fru->offset = 0;
+        fru->addr_off = offset;
+        fru->ubuf = buf;
+        bfa_fru_read_send(fru, BFI_TFRU_H2I_READ_REQ);
+
+        return BFA_STATUS_OK;
+}
+
+/*
+ * Process fru response messages upon receiving interrupts.
+ *
+ * @param[in] fruarg - fru structure
+ * @param[in] msg - message structure
+ */
+void
+bfa_fru_intr(void *fruarg, struct bfi_mbmsg_s *msg)
+{
+        struct bfa_fru_s *fru = fruarg;
+        struct bfi_fru_rsp_s *rsp = (struct bfi_fru_rsp_s *)msg;
+        u32 status;
+
+        bfa_trc(fru, msg->mh.msg_id);
+
+        if (!fru->op_busy) {
+                /*
+                 * receiving response after ioc failure
+                 */
+                bfa_trc(fru, 0x9999);
+                return;
+        }
+
+        switch (msg->mh.msg_id) {
+        case BFI_FRUVPD_I2H_WRITE_RSP:
+        case BFI_TFRU_I2H_WRITE_RSP:
+                status = be32_to_cpu(rsp->status);
+                bfa_trc(fru, status);
+
+                if (status != BFA_STATUS_OK || fru->residue == 0) {
+                        fru->status = status;
+                        fru->op_busy = 0;
+                        if (fru->cbfn)
+                                fru->cbfn(fru->cbarg, fru->status);
+                } else {
+                        bfa_trc(fru, fru->offset);
+                        if (msg->mh.msg_id == BFI_FRUVPD_I2H_WRITE_RSP)
+                                bfa_fru_write_send(fru,
+                                        BFI_FRUVPD_H2I_WRITE_REQ);
+                        else
+                                bfa_fru_write_send(fru,
+                                        BFI_TFRU_H2I_WRITE_REQ);
+                }
+                break;
+        case BFI_FRUVPD_I2H_READ_RSP:
+        case BFI_TFRU_I2H_READ_RSP:
+                status = be32_to_cpu(rsp->status);
+                bfa_trc(fru, status);
+
+                if (status != BFA_STATUS_OK) {
+                        fru->status = status;
+                        fru->op_busy = 0;
+                        if (fru->cbfn)
+                                fru->cbfn(fru->cbarg, fru->status);
+                } else {
+                        u32 len = be32_to_cpu(rsp->length);
+
+                        bfa_trc(fru, fru->offset);
+                        bfa_trc(fru, len);
+
+                        memcpy(fru->ubuf + fru->offset, fru->dbuf_kva, len);
+                        fru->residue -= len;
+                        fru->offset += len;
+
+                        if (fru->residue == 0) {
+                                fru->status = status;
+                                fru->op_busy = 0;
+                                if (fru->cbfn)
+                                        fru->cbfn(fru->cbarg, fru->status);
+                        } else {
+                                if (msg->mh.msg_id == BFI_FRUVPD_I2H_READ_RSP)
+                                        bfa_fru_read_send(fru,
+                                                BFI_FRUVPD_H2I_READ_REQ);
+                                else
+                                        bfa_fru_read_send(fru,
+                                                BFI_TFRU_H2I_READ_REQ);
+                        }
+                }
+                break;
+        default:
+                WARN_ON(1);
+        }
+}
diff --git a/drivers/scsi/bfa/bfa_ioc.h b/drivers/scsi/bfa/bfa_ioc.h
index b2856f96567c..23a90e7b7107 100644
--- a/drivers/scsi/bfa/bfa_ioc.h
+++ b/drivers/scsi/bfa/bfa_ioc.h
@@ -702,6 +702,55 @@ void bfa_phy_memclaim(struct bfa_phy_s *phy,
 void bfa_phy_intr(void *phyarg, struct bfi_mbmsg_s *msg);
 
 /*
+ * FRU module specific
+ */
+typedef void (*bfa_cb_fru_t) (void *cbarg, bfa_status_t status);
+
+struct bfa_fru_s {
+        struct bfa_ioc_s *ioc;          /* back pointer to ioc */
+        struct bfa_trc_mod_s *trcmod;   /* trace module */
+        u8              op_busy;        /* operation busy flag */
+        u8              rsv[3];
+        u32             residue;        /* residual length */
+        u32             offset;         /* offset */
+        bfa_status_t    status;         /* status */
+        u8              *dbuf_kva;      /* dma buf virtual address */
+        u64             dbuf_pa;        /* dma buf physical address */
+        struct bfa_reqq_wait_s reqq_wait; /* to wait for room in reqq */
+        bfa_cb_fru_t    cbfn;           /* user callback function */
+        void            *cbarg;         /* user callback arg */
+        u8              *ubuf;          /* user supplied buffer */
+        struct bfa_cb_qe_s      hcb_qe; /* comp: BFA callback qelem */
+        u32             addr_off;       /* fru address offset */
+        struct bfa_mbox_cmd_s mb;       /* mailbox */
+        struct bfa_ioc_notify_s ioc_notify; /* ioc event notify */
+        struct bfa_mem_dma_s    fru_dma;
+};
+
+#define BFA_FRU(__bfa)  (&(__bfa)->modules.fru)
+#define BFA_MEM_FRU_DMA(__bfa)  (&(BFA_FRU(__bfa)->fru_dma))
+
+bfa_status_t bfa_fruvpd_update(struct bfa_fru_s *fru,
+                        void *buf, u32 len, u32 offset,
+                        bfa_cb_fru_t cbfn, void *cbarg);
+bfa_status_t bfa_fruvpd_read(struct bfa_fru_s *fru,
+                        void *buf, u32 len, u32 offset,
+                        bfa_cb_fru_t cbfn, void *cbarg);
+bfa_status_t bfa_fruvpd_get_max_size(struct bfa_fru_s *fru, u32 *max_size);
+bfa_status_t bfa_tfru_write(struct bfa_fru_s *fru,
+                        void *buf, u32 len, u32 offset,
+                        bfa_cb_fru_t cbfn, void *cbarg);
+bfa_status_t bfa_tfru_read(struct bfa_fru_s *fru,
+                        void *buf, u32 len, u32 offset,
+                        bfa_cb_fru_t cbfn, void *cbarg);
+u32     bfa_fru_meminfo(bfa_boolean_t mincfg);
+void bfa_fru_attach(struct bfa_fru_s *fru, struct bfa_ioc_s *ioc,
+                void *dev, struct bfa_trc_mod_s *trcmod, bfa_boolean_t mincfg);
+void bfa_fru_memclaim(struct bfa_fru_s *fru,
+                u8 *dm_kva, u64 dm_pa, bfa_boolean_t mincfg);
+void bfa_fru_intr(void *fruarg, struct bfi_mbmsg_s *msg);
+
+/*
  * Driver Config( dconf) specific
  */
 #define BFI_DCONF_SIGNATURE     0xabcdabcd
@@ -716,6 +765,7 @@ struct bfa_dconf_hdr_s {
 struct bfa_dconf_s {
         struct bfa_dconf_hdr_s          hdr;
         struct bfa_lunmask_cfg_s        lun_mask;
+        struct bfa_throttle_cfg_s       throttle_cfg;
 };
 #pragma pack()
 
@@ -738,6 +788,8 @@ struct bfa_dconf_mod_s {
 #define bfa_dconf_read_data_valid(__bfa)        \
         (BFA_DCONF_MOD(__bfa)->read_data_valid)
 #define BFA_DCONF_UPDATE_TOV    5000    /* memtest timeout in msec */
+#define bfa_dconf_get_min_cfg(__bfa)    \
+        (BFA_DCONF_MOD(__bfa)->min_cfg)
 
 void    bfa_dconf_modinit(struct bfa_s *bfa);
 void    bfa_dconf_modexit(struct bfa_s *bfa);
@@ -761,7 +813,8 @@ bfa_status_t	bfa_dconf_update(struct bfa_s *bfa);
 #define bfa_ioc_maxfrsize(__ioc)        ((__ioc)->attr->maxfrsize)
 #define bfa_ioc_rx_bbcredit(__ioc)      ((__ioc)->attr->rx_bbcredit)
 #define bfa_ioc_speed_sup(__ioc)        \
-        BFI_ADAPTER_GETP(SPEED, (__ioc)->attr->adapter_prop)
+        ((bfa_ioc_is_cna(__ioc)) ? BFA_PORT_SPEED_10GBPS :      \
+         BFI_ADAPTER_GETP(SPEED, (__ioc)->attr->adapter_prop))
 #define bfa_ioc_get_nports(__ioc)       \
         BFI_ADAPTER_GETP(NPORTS, (__ioc)->attr->adapter_prop)
 
@@ -885,12 +938,12 @@ bfa_status_t bfa_ablk_port_config(struct bfa_ablk_s *ablk, int port,
                 enum bfa_mode_s mode, int max_pf, int max_vf,
                 bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_pf_create(struct bfa_ablk_s *ablk, u16 *pcifn,
-                u8 port, enum bfi_pcifn_class personality, int bw,
-                bfa_ablk_cbfn_t cbfn, void *cbarg);
+                u8 port, enum bfi_pcifn_class personality,
+                u16 bw_min, u16 bw_max, bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_pf_delete(struct bfa_ablk_s *ablk, int pcifn,
                 bfa_ablk_cbfn_t cbfn, void *cbarg);
-bfa_status_t bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, int bw,
-                bfa_ablk_cbfn_t cbfn, void *cbarg);
+bfa_status_t bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn,
+                u16 bw_min, u16 bw_max, bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_optrom_en(struct bfa_ablk_s *ablk,
                 bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_optrom_dis(struct bfa_ablk_s *ablk,
diff --git a/drivers/scsi/bfa/bfa_ioc_ct.c b/drivers/scsi/bfa/bfa_ioc_ct.c
index 2eb0c6a2938d..de4e726a1263 100644
--- a/drivers/scsi/bfa/bfa_ioc_ct.c
+++ b/drivers/scsi/bfa/bfa_ioc_ct.c
@@ -57,13 +57,6 @@ bfa_ioc_ct_firmware_lock(struct bfa_ioc_s *ioc)
         u32 usecnt;
         struct bfi_ioc_image_hdr_s fwhdr;
 
-        /*
-         * If bios boot (flash based) -- do not increment usage count
-         */
-        if (bfa_cb_image_get_size(bfa_ioc_asic_gen(ioc)) <
-                                                BFA_IOC_FWIMG_MINSZ)
-                return BFA_TRUE;
-
         bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
         usecnt = readl(ioc->ioc_regs.ioc_usage_reg);
 
@@ -115,13 +108,6 @@ bfa_ioc_ct_firmware_unlock(struct bfa_ioc_s *ioc)
         u32 usecnt;
 
         /*
-         * If bios boot (flash based) -- do not decrement usage count
-         */
-        if (bfa_cb_image_get_size(bfa_ioc_asic_gen(ioc)) <
-                                                BFA_IOC_FWIMG_MINSZ)
-                return;
-
-        /*
          * decrement usage count
          */
         bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
@@ -400,13 +386,12 @@ static void
 bfa_ioc_ct_ownership_reset(struct bfa_ioc_s *ioc)
 {
 
-        if (bfa_ioc_is_cna(ioc)) {
-                bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
-                writel(0, ioc->ioc_regs.ioc_usage_reg);
-                readl(ioc->ioc_regs.ioc_usage_sem_reg);
-                writel(1, ioc->ioc_regs.ioc_usage_sem_reg);
-        }
+        bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
+        writel(0, ioc->ioc_regs.ioc_usage_reg);
+        readl(ioc->ioc_regs.ioc_usage_sem_reg);
+        writel(1, ioc->ioc_regs.ioc_usage_sem_reg);
 
+        writel(0, ioc->ioc_regs.ioc_fail_sync);
         /*
          * Read the hw sem reg to make sure that it is locked
          * before we clear it. If it is not locked, writing 1
@@ -759,25 +744,6 @@ bfa_ioc_ct2_mem_init(void __iomem *rb)
 void
 bfa_ioc_ct2_mac_reset(void __iomem *rb)
 {
-        u32     r32;
-
-        bfa_ioc_ct2_sclk_init(rb);
-        bfa_ioc_ct2_lclk_init(rb);
-
-        /*
-         * release soft reset on s_clk & l_clk
-         */
-        r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
-        writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
-                (rb + CT2_APP_PLL_SCLK_CTL_REG));
-
-        /*
-         * release soft reset on s_clk & l_clk
-         */
-        r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
-        writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
-                (rb + CT2_APP_PLL_LCLK_CTL_REG));
-
         /* put port0, port1 MAC & AHB in reset */
         writel((__CSI_MAC_RESET | __CSI_MAC_AHB_RESET),
                 rb + CT2_CSI_MAC_CONTROL_REG(0));
@@ -785,8 +751,21 @@ bfa_ioc_ct2_mac_reset(void __iomem *rb)
                 rb + CT2_CSI_MAC_CONTROL_REG(1));
 }
 
+static void
+bfa_ioc_ct2_enable_flash(void __iomem *rb)
+{
+        u32 r32;
+
+        r32 = readl((rb + PSS_GPIO_OUT_REG));
+        writel(r32 & ~1, (rb + PSS_GPIO_OUT_REG));
+        r32 = readl((rb + PSS_GPIO_OE_REG));
+        writel(r32 | 1, (rb + PSS_GPIO_OE_REG));
+}
+
 #define CT2_NFC_MAX_DELAY       1000
-#define CT2_NFC_VER_VALID       0x143
+#define CT2_NFC_PAUSE_MAX_DELAY 4000
+#define CT2_NFC_VER_VALID       0x147
+#define CT2_NFC_STATE_RUNNING   0x20000001
 #define BFA_IOC_PLL_POLL        1000000
 
 static bfa_boolean_t
@@ -802,6 +781,20 @@ bfa_ioc_ct2_nfc_halted(void __iomem *rb)
 }
 
 static void
+bfa_ioc_ct2_nfc_halt(void __iomem *rb)
+{
+        int     i;
+
+        writel(__HALT_NFC_CONTROLLER, rb + CT2_NFC_CSR_SET_REG);
+        for (i = 0; i < CT2_NFC_MAX_DELAY; i++) {
+                if (bfa_ioc_ct2_nfc_halted(rb))
+                        break;
+                udelay(1000);
+        }
+        WARN_ON(!bfa_ioc_ct2_nfc_halted(rb));
+}
+
+static void
 bfa_ioc_ct2_nfc_resume(void __iomem *rb)
 {
         u32     r32;
@@ -817,105 +810,142 @@ bfa_ioc_ct2_nfc_resume(void __iomem *rb)
         WARN_ON(1);
 }
 
-bfa_status_t
-bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode mode)
+static void
+bfa_ioc_ct2_clk_reset(void __iomem *rb)
 {
-        u32 wgn, r32, nfc_ver, i;
+        u32 r32;
 
-        wgn = readl(rb + CT2_WGN_STATUS);
-        nfc_ver = readl(rb + CT2_RSC_GPR15_REG);
+        bfa_ioc_ct2_sclk_init(rb);
+        bfa_ioc_ct2_lclk_init(rb);
 
-        if ((wgn == (__A2T_AHB_LOAD | __WGN_READY)) &&
-            (nfc_ver >= CT2_NFC_VER_VALID)) {
-                if (bfa_ioc_ct2_nfc_halted(rb))
-                        bfa_ioc_ct2_nfc_resume(rb);
+        /*
+         * release soft reset on s_clk & l_clk
+         */
+        r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+        writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
+                        (rb + CT2_APP_PLL_SCLK_CTL_REG));
 
-                writel(__RESET_AND_START_SCLK_LCLK_PLLS,
-                       rb + CT2_CSI_FW_CTL_SET_REG);
+        r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
+        writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
+                        (rb + CT2_APP_PLL_LCLK_CTL_REG));
 
-                for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
-                        r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
-                        if (r32 & __RESET_AND_START_SCLK_LCLK_PLLS)
-                                break;
-                }
+}
 
-                WARN_ON(!(r32 & __RESET_AND_START_SCLK_LCLK_PLLS));
+static void
+bfa_ioc_ct2_nfc_clk_reset(void __iomem *rb)
+{
+        u32 r32, i;
 
-                for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
-                        r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
-                        if (!(r32 & __RESET_AND_START_SCLK_LCLK_PLLS))
-                                break;
-                }
+        r32 = readl((rb + PSS_CTL_REG));
+        r32 |= (__PSS_LPU0_RESET | __PSS_LPU1_RESET);
+        writel(r32, (rb + PSS_CTL_REG));
+
+        writel(__RESET_AND_START_SCLK_LCLK_PLLS, rb + CT2_CSI_FW_CTL_SET_REG);
 
-                WARN_ON(r32 & __RESET_AND_START_SCLK_LCLK_PLLS);
+        for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
+                r32 = readl(rb + CT2_NFC_FLASH_STS_REG);
+
+                if ((r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS))
+                        break;
+        }
+        WARN_ON(!(r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS));
+
+        for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
+                r32 = readl(rb + CT2_NFC_FLASH_STS_REG);
+
+                if (!(r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS))
+                        break;
+        }
+        WARN_ON((r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS));
+
+        r32 = readl(rb + CT2_CSI_FW_CTL_REG);
+        WARN_ON((r32 & __RESET_AND_START_SCLK_LCLK_PLLS));
+}
+
+static void
+bfa_ioc_ct2_wait_till_nfc_running(void __iomem *rb)
+{
+        u32 r32;
+        int i;
+
+        if (bfa_ioc_ct2_nfc_halted(rb))
+                bfa_ioc_ct2_nfc_resume(rb);
+        for (i = 0; i < CT2_NFC_PAUSE_MAX_DELAY; i++) {
+                r32 = readl(rb + CT2_NFC_STS_REG);
+                if (r32 == CT2_NFC_STATE_RUNNING)
+                        return;
                 udelay(1000);
+        }
 
-                r32 = readl(rb + CT2_CSI_FW_CTL_REG);
-                WARN_ON(r32 & __RESET_AND_START_SCLK_LCLK_PLLS);
-        } else {
-                writel(__HALT_NFC_CONTROLLER, rb + CT2_NFC_CSR_SET_REG);
-                for (i = 0; i < CT2_NFC_MAX_DELAY; i++) {
-                        r32 = readl(rb + CT2_NFC_CSR_SET_REG);
-                        if (r32 & __NFC_CONTROLLER_HALTED)
-                                break;
-                        udelay(1000);
-                }
+        r32 = readl(rb + CT2_NFC_STS_REG);
+        WARN_ON(!(r32 == CT2_NFC_STATE_RUNNING));
+}
 
-                bfa_ioc_ct2_mac_reset(rb);
-                bfa_ioc_ct2_sclk_init(rb);
-                bfa_ioc_ct2_lclk_init(rb);
+bfa_status_t
+bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode mode)
+{
+        u32 wgn, r32, nfc_ver;
 
-                /*
-                 * release soft reset on s_clk & l_clk
-                 */
-                r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
-                writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
-                       (rb + CT2_APP_PLL_SCLK_CTL_REG));
+        wgn = readl(rb + CT2_WGN_STATUS);
 
+        if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
                 /*
-                 * release soft reset on s_clk & l_clk
+                 * If flash is corrupted, enable flash explicitly
                  */
-                r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
-                writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
-                      (rb + CT2_APP_PLL_LCLK_CTL_REG));
-        }
+                bfa_ioc_ct2_clk_reset(rb);
+                bfa_ioc_ct2_enable_flash(rb);
 
-        /*
-         * Announce flash device presence, if flash was corrupted.
-         */
-        if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
-                r32 = readl(rb + PSS_GPIO_OUT_REG);
-                writel(r32 & ~1, (rb + PSS_GPIO_OUT_REG));
-                r32 = readl(rb + PSS_GPIO_OE_REG);
-                writel(r32 | 1, (rb + PSS_GPIO_OE_REG));
+                bfa_ioc_ct2_mac_reset(rb);
+
+                bfa_ioc_ct2_clk_reset(rb);
+                bfa_ioc_ct2_enable_flash(rb);
+
+        } else {
+                nfc_ver = readl(rb + CT2_RSC_GPR15_REG);
+
+                if ((nfc_ver >= CT2_NFC_VER_VALID) &&
+                    (wgn == (__A2T_AHB_LOAD | __WGN_READY))) {
+
+                        bfa_ioc_ct2_wait_till_nfc_running(rb);
+
+                        bfa_ioc_ct2_nfc_clk_reset(rb);
+                } else {
+                        bfa_ioc_ct2_nfc_halt(rb);
+
+                        bfa_ioc_ct2_clk_reset(rb);
+                        bfa_ioc_ct2_mac_reset(rb);
+                        bfa_ioc_ct2_clk_reset(rb);
+
+                }
         }
 
         /*
          * Mask the interrupts and clear any
-         * pending interrupts.
+         * pending interrupts left by BIOS/EFI
          */
+
         writel(1, (rb + CT2_LPU0_HOSTFN_MBOX0_MSK));
         writel(1, (rb + CT2_LPU1_HOSTFN_MBOX0_MSK));
 
         /* For first time initialization, no need to clear interrupts */
         r32 = readl(rb + HOST_SEM5_REG);
         if (r32 & 0x1) {
-                r32 = readl(rb + CT2_LPU0_HOSTFN_CMD_STAT);
+                r32 = readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
                 if (r32 == 1) {
-                        writel(1, rb + CT2_LPU0_HOSTFN_CMD_STAT);
+                        writel(1, (rb + CT2_LPU0_HOSTFN_CMD_STAT));
                         readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
                 }
-                r32 = readl(rb + CT2_LPU1_HOSTFN_CMD_STAT);
+                r32 = readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
                 if (r32 == 1) {
-                        writel(1, rb + CT2_LPU1_HOSTFN_CMD_STAT);
-                        readl(rb + CT2_LPU1_HOSTFN_CMD_STAT);
+                        writel(1, (rb + CT2_LPU1_HOSTFN_CMD_STAT));
+                        readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
                 }
         }
 
         bfa_ioc_ct2_mem_init(rb);
 
-        writel(BFI_IOC_UNINIT, rb + CT2_BFA_IOC0_STATE_REG);
-        writel(BFI_IOC_UNINIT, rb + CT2_BFA_IOC1_STATE_REG);
+        writel(BFI_IOC_UNINIT, (rb + CT2_BFA_IOC0_STATE_REG));
+        writel(BFI_IOC_UNINIT, (rb + CT2_BFA_IOC1_STATE_REG));
 
         return BFA_STATUS_OK;
 }
diff --git a/drivers/scsi/bfa/bfa_modules.h b/drivers/scsi/bfa/bfa_modules.h
index 189fff71e3c2..a14c784ff3fc 100644
--- a/drivers/scsi/bfa/bfa_modules.h
+++ b/drivers/scsi/bfa/bfa_modules.h
@@ -45,6 +45,7 @@ struct bfa_modules_s {
         struct bfa_diag_s       diag_mod;       /*  diagnostics module  */
         struct bfa_phy_s        phy;            /*  phy module          */
         struct bfa_dconf_mod_s  dconf_mod;      /*  DCONF common module */
+        struct bfa_fru_s        fru;            /*  fru module          */
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_port.c b/drivers/scsi/bfa/bfa_port.c
index 95e4ad8759ac..8ea7697deb9b 100644
--- a/drivers/scsi/bfa/bfa_port.c
+++ b/drivers/scsi/bfa/bfa_port.c
@@ -250,6 +250,12 @@ bfa_port_enable(struct bfa_port_s *port, bfa_port_endis_cbfn_t cbfn,
                 return BFA_STATUS_IOC_FAILURE;
         }
 
+        /* if port is d-port enabled, return error */
+        if (port->dport_enabled) {
+                bfa_trc(port, BFA_STATUS_DPORT_ERR);
+                return BFA_STATUS_DPORT_ERR;
+        }
+
         if (port->endis_pending) {
                 bfa_trc(port, BFA_STATUS_DEVBUSY);
                 return BFA_STATUS_DEVBUSY;
@@ -300,6 +306,12 @@ bfa_port_disable(struct bfa_port_s *port, bfa_port_endis_cbfn_t cbfn,
                 return BFA_STATUS_IOC_FAILURE;
         }
 
+        /* if port is d-port enabled, return error */
+        if (port->dport_enabled) {
+                bfa_trc(port, BFA_STATUS_DPORT_ERR);
+                return BFA_STATUS_DPORT_ERR;
+        }
+
         if (port->endis_pending) {
                 bfa_trc(port, BFA_STATUS_DEVBUSY);
                 return BFA_STATUS_DEVBUSY;
@@ -431,6 +443,10 @@ bfa_port_notify(void *arg, enum bfa_ioc_event_e event)
                         port->endis_cbfn = NULL;
                         port->endis_pending = BFA_FALSE;
                 }
+
+                /* clear D-port mode */
+                if (port->dport_enabled)
+                        bfa_port_set_dportenabled(port, BFA_FALSE);
                 break;
         default:
                 break;
@@ -467,6 +483,7 @@ bfa_port_attach(struct bfa_port_s *port, struct bfa_ioc_s *ioc,
         port->stats_cbfn = NULL;
         port->endis_cbfn = NULL;
         port->pbc_disabled = BFA_FALSE;
+        port->dport_enabled = BFA_FALSE;
 
         bfa_ioc_mbox_regisr(port->ioc, BFI_MC_PORT, bfa_port_isr, port);
         bfa_q_qe_init(&port->ioc_notify);
@@ -483,6 +500,21 @@ bfa_port_attach(struct bfa_port_s *port, struct bfa_ioc_s *ioc,
 }
 
 /*
+ * bfa_port_set_dportenabled();
+ *
+ * Port module- set pbc disabled flag
+ *
+ * @param[in] port - Pointer to the Port module data structure
+ *
+ * @return void
+ */
+void
+bfa_port_set_dportenabled(struct bfa_port_s *port, bfa_boolean_t enabled)
+{
+        port->dport_enabled = enabled;
+}
+
+/*
  *      CEE module specific definitions
  */
 
diff --git a/drivers/scsi/bfa/bfa_port.h b/drivers/scsi/bfa/bfa_port.h
index 947f897328d6..2fcab6bc6280 100644
--- a/drivers/scsi/bfa/bfa_port.h
+++ b/drivers/scsi/bfa/bfa_port.h
@@ -45,6 +45,7 @@ struct bfa_port_s {
         bfa_status_t                    endis_status;
         struct bfa_ioc_notify_s         ioc_notify;
         bfa_boolean_t                   pbc_disabled;
+        bfa_boolean_t                   dport_enabled;
         struct bfa_mem_dma_s            port_dma;
 };
 
@@ -66,6 +67,8 @@ bfa_status_t bfa_port_disable(struct bfa_port_s *port,
 u32     bfa_port_meminfo(void);
 void         bfa_port_mem_claim(struct bfa_port_s *port,
                                  u8 *dma_kva, u64 dma_pa);
+void    bfa_port_set_dportenabled(struct bfa_port_s *port,
+                                  bfa_boolean_t enabled);
 
 /*
  * CEE declaration
diff --git a/drivers/scsi/bfa/bfa_svc.c b/drivers/scsi/bfa/bfa_svc.c
index b2538d60db34..299c1c889b33 100644
--- a/drivers/scsi/bfa/bfa_svc.c
+++ b/drivers/scsi/bfa/bfa_svc.c
@@ -67,6 +67,9 @@ enum bfa_fcport_sm_event {
         BFA_FCPORT_SM_LINKDOWN  = 7,    /*  firmware linkup down        */
         BFA_FCPORT_SM_QRESUME   = 8,    /*  CQ space available  */
         BFA_FCPORT_SM_HWFAIL    = 9,    /*  IOC h/w failure             */
+        BFA_FCPORT_SM_DPORTENABLE = 10, /*  enable dport      */
+        BFA_FCPORT_SM_DPORTDISABLE = 11,/*  disable dport     */
+        BFA_FCPORT_SM_FAA_MISCONFIG = 12,       /* FAA misconfiguratin */
 };
 
 /*
@@ -197,6 +200,10 @@ static void     bfa_fcport_sm_iocdown(struct bfa_fcport_s *fcport,
                                         enum bfa_fcport_sm_event event);
 static void     bfa_fcport_sm_iocfail(struct bfa_fcport_s *fcport,
                                         enum bfa_fcport_sm_event event);
+static void     bfa_fcport_sm_dport(struct bfa_fcport_s *fcport,
+                                        enum bfa_fcport_sm_event event);
+static void     bfa_fcport_sm_faa_misconfig(struct bfa_fcport_s *fcport,
+                                        enum bfa_fcport_sm_event event);
 
 static void     bfa_fcport_ln_sm_dn(struct bfa_fcport_ln_s *ln,
                                         enum bfa_fcport_ln_sm_event event);
@@ -226,6 +233,8 @@ static struct bfa_sm_table_s hal_port_sm_table[] = {
         {BFA_SM(bfa_fcport_sm_stopped), BFA_PORT_ST_STOPPED},
         {BFA_SM(bfa_fcport_sm_iocdown), BFA_PORT_ST_IOCDOWN},
         {BFA_SM(bfa_fcport_sm_iocfail), BFA_PORT_ST_IOCDOWN},
+        {BFA_SM(bfa_fcport_sm_dport), BFA_PORT_ST_DPORT},
+        {BFA_SM(bfa_fcport_sm_faa_misconfig), BFA_PORT_ST_FAA_MISCONFIG},
 };
 
 
@@ -1244,6 +1253,12 @@ bfa_lps_sm_init(struct bfa_lps_s *lps, enum bfa_lps_event event)
                  * Just ignore
                  */
                 break;
+        case BFA_LPS_SM_SET_N2N_PID:
+                /*
+                 * When topology is set to loop, bfa_lps_set_n2n_pid() sends
+                 * this event. Ignore this event.
+                 */
+                break;
 
         default:
                 bfa_sm_fault(lps->bfa, event);
@@ -1261,6 +1276,7 @@ bfa_lps_sm_login(struct bfa_lps_s *lps, enum bfa_lps_event event)
 
         switch (event) {
         case BFA_LPS_SM_FWRSP:
+        case BFA_LPS_SM_OFFLINE:
                 if (lps->status == BFA_STATUS_OK) {
                         bfa_sm_set_state(lps, bfa_lps_sm_online);
                         if (lps->fdisc)
@@ -1289,7 +1305,6 @@ bfa_lps_sm_login(struct bfa_lps_s *lps, enum bfa_lps_event event)
                 bfa_lps_login_comp(lps);
                 break;
 
-        case BFA_LPS_SM_OFFLINE:
         case BFA_LPS_SM_DELETE:
                 bfa_sm_set_state(lps, bfa_lps_sm_init);
                 break;
@@ -2169,6 +2184,12 @@ bfa_fcport_sm_enabling_qwait(struct bfa_fcport_s *fcport,
                 bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
                 break;
 
+        case BFA_FCPORT_SM_FAA_MISCONFIG:
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+                bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+                break;
+
         default:
                 bfa_sm_fault(fcport->bfa, event);
         }
@@ -2225,6 +2246,12 @@ bfa_fcport_sm_enabling(struct bfa_fcport_s *fcport,
                 bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
                 break;
 
+        case BFA_FCPORT_SM_FAA_MISCONFIG:
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+                bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+                break;
+
         default:
                 bfa_sm_fault(fcport->bfa, event);
         }
@@ -2250,11 +2277,11 @@ bfa_fcport_sm_linkdown(struct bfa_fcport_s *fcport,
                 if (!bfa_ioc_get_fcmode(&fcport->bfa->ioc)) {
 
                         bfa_trc(fcport->bfa,
-                                pevent->link_state.vc_fcf.fcf.fipenabled);
+                                pevent->link_state.attr.vc_fcf.fcf.fipenabled);
                         bfa_trc(fcport->bfa,
-                                pevent->link_state.vc_fcf.fcf.fipfailed);
+                                pevent->link_state.attr.vc_fcf.fcf.fipfailed);
 
-                        if (pevent->link_state.vc_fcf.fcf.fipfailed)
+                        if (pevent->link_state.attr.vc_fcf.fcf.fipfailed)
                                 bfa_plog_str(fcport->bfa->plog, BFA_PL_MID_HAL,
                                         BFA_PL_EID_FIP_FCF_DISC, 0,
                                         "FIP FCF Discovery Failed");
@@ -2311,6 +2338,12 @@ bfa_fcport_sm_linkdown(struct bfa_fcport_s *fcport,
                 bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
                 break;
 
+        case BFA_FCPORT_SM_FAA_MISCONFIG:
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+                bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+                break;
+
         default:
                 bfa_sm_fault(fcport->bfa, event);
         }
@@ -2404,6 +2437,12 @@ bfa_fcport_sm_linkup(struct bfa_fcport_s *fcport,
                 }
                 break;
 
+        case BFA_FCPORT_SM_FAA_MISCONFIG:
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+                bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+                break;
+
         default:
                 bfa_sm_fault(fcport->bfa, event);
         }
@@ -2449,6 +2488,12 @@ bfa_fcport_sm_disabling_qwait(struct bfa_fcport_s *fcport,
                 bfa_reqq_wcancel(&fcport->reqq_wait);
                 break;
 
+        case BFA_FCPORT_SM_FAA_MISCONFIG:
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+                bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+                break;
+
         default:
                 bfa_sm_fault(fcport->bfa, event);
         }
@@ -2600,6 +2645,10 @@ bfa_fcport_sm_disabled(struct bfa_fcport_s *fcport,
                 bfa_sm_set_state(fcport, bfa_fcport_sm_iocfail);
                 break;
 
+        case BFA_FCPORT_SM_DPORTENABLE:
+                bfa_sm_set_state(fcport, bfa_fcport_sm_dport);
+                break;
+
         default:
                 bfa_sm_fault(fcport->bfa, event);
         }
@@ -2680,6 +2729,81 @@ bfa_fcport_sm_iocfail(struct bfa_fcport_s *fcport,
         }
 }
 
+static void
+bfa_fcport_sm_dport(struct bfa_fcport_s *fcport, enum bfa_fcport_sm_event event)
+{
+        bfa_trc(fcport->bfa, event);
+
+        switch (event) {
+        case BFA_FCPORT_SM_DPORTENABLE:
+        case BFA_FCPORT_SM_DISABLE:
+        case BFA_FCPORT_SM_ENABLE:
+        case BFA_FCPORT_SM_START:
+                /*
+                 * Ignore event for a port that is dport
+                 */
+                break;
+
+        case BFA_FCPORT_SM_STOP:
+                bfa_sm_set_state(fcport, bfa_fcport_sm_stopped);
+                break;
+
+        case BFA_FCPORT_SM_HWFAIL:
+                bfa_sm_set_state(fcport, bfa_fcport_sm_iocfail);
+                break;
+
+        case BFA_FCPORT_SM_DPORTDISABLE:
+                bfa_sm_set_state(fcport, bfa_fcport_sm_disabled);
+                break;
+
+        default:
+                bfa_sm_fault(fcport->bfa, event);
+        }
+}
+
+static void
+bfa_fcport_sm_faa_misconfig(struct bfa_fcport_s *fcport,
+                            enum bfa_fcport_sm_event event)
+{
+        bfa_trc(fcport->bfa, event);
+
+        switch (event) {
+        case BFA_FCPORT_SM_DPORTENABLE:
+        case BFA_FCPORT_SM_ENABLE:
+        case BFA_FCPORT_SM_START:
+                /*
+                 * Ignore event for a port as there is FAA misconfig
+                 */
+                break;
+
+        case BFA_FCPORT_SM_DISABLE:
+                if (bfa_fcport_send_disable(fcport))
+                        bfa_sm_set_state(fcport, bfa_fcport_sm_disabling);
+                else
+                        bfa_sm_set_state(fcport, bfa_fcport_sm_disabling_qwait);
+
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_scn(fcport, BFA_PORT_LINKDOWN, BFA_FALSE);
+                bfa_plog_str(fcport->bfa->plog, BFA_PL_MID_HAL,
+                             BFA_PL_EID_PORT_DISABLE, 0, "Port Disable");
+                bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISABLE);
+                break;
+
+        case BFA_FCPORT_SM_STOP:
+                bfa_sm_set_state(fcport, bfa_fcport_sm_stopped);
+                break;
+
+        case BFA_FCPORT_SM_HWFAIL:
+                bfa_fcport_reset_linkinfo(fcport);
+                bfa_fcport_scn(fcport, BFA_PORT_LINKDOWN, BFA_FALSE);
+                bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
+                break;
+
+        default:
+                bfa_sm_fault(fcport->bfa, event);
+        }
+}
+
 /*
  * Link state is down
  */
@@ -2943,6 +3067,7 @@ bfa_fcport_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
          */
         do_gettimeofday(&tv);
         fcport->stats_reset_time = tv.tv_sec;
+        fcport->stats_dma_ready = BFA_FALSE;
 
         /*
          * initialize and set default configuration
@@ -2953,6 +3078,9 @@ bfa_fcport_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
         port_cfg->maxfrsize = 0;
 
         port_cfg->trl_def_speed = BFA_PORT_SPEED_1GBPS;
+        port_cfg->qos_bw.high = BFA_QOS_BW_HIGH;
+        port_cfg->qos_bw.med = BFA_QOS_BW_MED;
+        port_cfg->qos_bw.low = BFA_QOS_BW_LOW;
 
         INIT_LIST_HEAD(&fcport->stats_pending_q);
         INIT_LIST_HEAD(&fcport->statsclr_pending_q);
@@ -2996,6 +3124,21 @@ bfa_fcport_iocdisable(struct bfa_s *bfa)
         bfa_trunk_iocdisable(bfa);
 }
 
+/*
+ * Update loop info in fcport for SCN online
+ */
+static void
+bfa_fcport_update_loop_info(struct bfa_fcport_s *fcport,
+                        struct bfa_fcport_loop_info_s *loop_info)
+{
+        fcport->myalpa = loop_info->myalpa;
+        fcport->alpabm_valid =
+                        loop_info->alpabm_val;
+        memcpy(fcport->alpabm.alpa_bm,
+                        loop_info->alpabm.alpa_bm,
+                        sizeof(struct fc_alpabm_s));
+}
+
 static void
 bfa_fcport_update_linkinfo(struct bfa_fcport_s *fcport)
 {
@@ -3005,12 +3148,15 @@ bfa_fcport_update_linkinfo(struct bfa_fcport_s *fcport)
         fcport->speed = pevent->link_state.speed;
         fcport->topology = pevent->link_state.topology;
 
-        if (fcport->topology == BFA_PORT_TOPOLOGY_LOOP)
-                fcport->myalpa = 0;
+        if (fcport->topology == BFA_PORT_TOPOLOGY_LOOP) {
+                bfa_fcport_update_loop_info(fcport,
+                                &pevent->link_state.attr.loop_info);
+                return;
+        }
 
         /* QoS Details */
         fcport->qos_attr = pevent->link_state.qos_attr;
-        fcport->qos_vc_attr = pevent->link_state.vc_fcf.qos_vc_attr;
+        fcport->qos_vc_attr = pevent->link_state.attr.vc_fcf.qos_vc_attr;
 
         /*
          * update trunk state if applicable
@@ -3019,7 +3165,8 @@ bfa_fcport_update_linkinfo(struct bfa_fcport_s *fcport)
                 trunk->attr.state = BFA_TRUNK_DISABLED;
 
         /* update FCoE specific */
-        fcport->fcoe_vlan = be16_to_cpu(pevent->link_state.vc_fcf.fcf.vlan);
+        fcport->fcoe_vlan =
+                be16_to_cpu(pevent->link_state.attr.vc_fcf.fcf.vlan);
 
         bfa_trc(fcport->bfa, fcport->speed);
         bfa_trc(fcport->bfa, fcport->topology);
@@ -3453,6 +3600,7 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
         case BFI_FCPORT_I2H_ENABLE_RSP:
                 if (fcport->msgtag == i2hmsg.penable_rsp->msgtag) {
 
+                        fcport->stats_dma_ready = BFA_TRUE;
                         if (fcport->use_flash_cfg) {
                                 fcport->cfg = i2hmsg.penable_rsp->port_cfg;
                                 fcport->cfg.maxfrsize =
@@ -3468,6 +3616,8 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
                                 else
                                         fcport->trunk.attr.state =
                                                 BFA_TRUNK_DISABLED;
+                                fcport->qos_attr.qos_bw =
+                                        i2hmsg.penable_rsp->port_cfg.qos_bw;
                                 fcport->use_flash_cfg = BFA_FALSE;
                         }
 
@@ -3476,6 +3626,9 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
                         else
                                 fcport->qos_attr.state = BFA_QOS_DISABLED;
 
+                        fcport->qos_attr.qos_bw_op =
+                                        i2hmsg.penable_rsp->port_cfg.qos_bw;
+
                         bfa_sm_send_event(fcport, BFA_FCPORT_SM_FWRSP);
                 }
                 break;
@@ -3488,8 +3641,17 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
         case BFI_FCPORT_I2H_EVENT:
                 if (i2hmsg.event->link_state.linkstate == BFA_PORT_LINKUP)
                         bfa_sm_send_event(fcport, BFA_FCPORT_SM_LINKUP);
-                else
-                        bfa_sm_send_event(fcport, BFA_FCPORT_SM_LINKDOWN);
+                else {
+                        if (i2hmsg.event->link_state.linkstate_rsn ==
+                            BFA_PORT_LINKSTATE_RSN_FAA_MISCONFIG)
+                                bfa_sm_send_event(fcport,
+                                                  BFA_FCPORT_SM_FAA_MISCONFIG);
+                        else
+                                bfa_sm_send_event(fcport,
+                                                  BFA_FCPORT_SM_LINKDOWN);
+                }
+                fcport->qos_attr.qos_bw_op =
+                                i2hmsg.event->link_state.qos_attr.qos_bw_op;
                 break;
 
         case BFI_FCPORT_I2H_TRUNK_SCN:
@@ -3609,6 +3771,9 @@ bfa_fcport_cfg_speed(struct bfa_s *bfa, enum bfa_port_speed speed)
 
         if (fcport->cfg.trunked == BFA_TRUE)
                 return BFA_STATUS_TRUNK_ENABLED;
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                        (speed == BFA_PORT_SPEED_16GBPS))
+                return BFA_STATUS_UNSUPP_SPEED;
         if ((speed != BFA_PORT_SPEED_AUTO) && (speed > fcport->speed_sup)) {
                 bfa_trc(bfa, fcport->speed_sup);
                 return BFA_STATUS_UNSUPP_SPEED;
@@ -3663,7 +3828,26 @@ bfa_fcport_cfg_topology(struct bfa_s *bfa, enum bfa_port_topology topology)
 
         switch (topology) {
         case BFA_PORT_TOPOLOGY_P2P:
+                break;
+
         case BFA_PORT_TOPOLOGY_LOOP:
+                if ((bfa_fcport_is_qos_enabled(bfa) != BFA_FALSE) ||
+                        (fcport->qos_attr.state != BFA_QOS_DISABLED))
+                        return BFA_STATUS_ERROR_QOS_ENABLED;
+                if (fcport->cfg.ratelimit != BFA_FALSE)
+                        return BFA_STATUS_ERROR_TRL_ENABLED;
+                if ((bfa_fcport_is_trunk_enabled(bfa) != BFA_FALSE) ||
+                        (fcport->trunk.attr.state != BFA_TRUNK_DISABLED))
+                        return BFA_STATUS_ERROR_TRUNK_ENABLED;
+                if ((bfa_fcport_get_speed(bfa) == BFA_PORT_SPEED_16GBPS) ||
+                        (fcport->cfg.speed == BFA_PORT_SPEED_16GBPS))
+                        return BFA_STATUS_UNSUPP_SPEED;
+                if (bfa_mfg_is_mezz(bfa->ioc.attr->card_type))
+                        return BFA_STATUS_LOOP_UNSUPP_MEZZ;
+                if (bfa_fcport_is_dport(bfa) != BFA_FALSE)
+                        return BFA_STATUS_DPORT_ERR;
+                break;
+
         case BFA_PORT_TOPOLOGY_AUTO:
                 break;
 
@@ -3686,6 +3870,17 @@ bfa_fcport_get_topology(struct bfa_s *bfa)
         return fcport->topology;
 }
 
+/**
+ * Get config topology.
+ */
+enum bfa_port_topology
+bfa_fcport_get_cfg_topology(struct bfa_s *bfa)
+{
+        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+
+        return fcport->cfg.topology;
+}
+
 bfa_status_t
 bfa_fcport_cfg_hardalpa(struct bfa_s *bfa, u8 alpa)
 {
@@ -3761,9 +3956,11 @@ bfa_fcport_get_maxfrsize(struct bfa_s *bfa)
 u8
 bfa_fcport_get_rx_bbcredit(struct bfa_s *bfa)
 {
-        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+        if (bfa_fcport_get_topology(bfa) != BFA_PORT_TOPOLOGY_LOOP)
+                return (BFA_FCPORT_MOD(bfa))->cfg.rx_bbcredit;
 
-        return fcport->cfg.rx_bbcredit;
+        else
+                return 0;
 }
 
 void
@@ -3850,8 +4047,9 @@ bfa_fcport_get_stats(struct bfa_s *bfa, struct bfa_cb_pending_q_s *cb)
 {
         struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
 
-        if (bfa_ioc_is_disabled(&bfa->ioc))
-                return BFA_STATUS_IOC_DISABLED;
+        if (!bfa_iocfc_is_operational(bfa) ||
+            !fcport->stats_dma_ready)
+                return BFA_STATUS_IOC_NON_OP;
 
         if (!list_empty(&fcport->statsclr_pending_q))
                 return BFA_STATUS_DEVBUSY;
@@ -3876,6 +4074,10 @@ bfa_fcport_clear_stats(struct bfa_s *bfa, struct bfa_cb_pending_q_s *cb)
 {
         struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
 
+        if (!bfa_iocfc_is_operational(bfa) ||
+            !fcport->stats_dma_ready)
+                return BFA_STATUS_IOC_NON_OP;
+
         if (!list_empty(&fcport->stats_pending_q))
                 return BFA_STATUS_DEVBUSY;
 
@@ -3905,6 +4107,40 @@ bfa_fcport_is_disabled(struct bfa_s *bfa)
 }
 
 bfa_boolean_t
+bfa_fcport_is_dport(struct bfa_s *bfa)
+{
+        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+
+        return (bfa_sm_to_state(hal_port_sm_table, fcport->sm) ==
+                BFA_PORT_ST_DPORT);
+}
+
+bfa_status_t
+bfa_fcport_set_qos_bw(struct bfa_s *bfa, struct bfa_qos_bw_s *qos_bw)
+{
+        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+        enum bfa_ioc_type_e ioc_type = bfa_get_type(bfa);
+
+        bfa_trc(bfa, ioc_type);
+
+        if ((qos_bw->high == 0) || (qos_bw->med == 0) || (qos_bw->low == 0))
+                return BFA_STATUS_QOS_BW_INVALID;
+
+        if ((qos_bw->high + qos_bw->med + qos_bw->low) != 100)
+                return BFA_STATUS_QOS_BW_INVALID;
+
+        if ((qos_bw->med > qos_bw->high) || (qos_bw->low > qos_bw->med) ||
+            (qos_bw->low > qos_bw->high))
+                return BFA_STATUS_QOS_BW_INVALID;
+
+        if ((ioc_type == BFA_IOC_TYPE_FC) &&
+            (fcport->cfg.topology != BFA_PORT_TOPOLOGY_LOOP))
+                fcport->cfg.qos_bw = *qos_bw;
+
+        return BFA_STATUS_OK;
+}
+
+bfa_boolean_t
 bfa_fcport_is_ratelim(struct bfa_s *bfa)
 {
         struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
@@ -3981,6 +4217,26 @@ bfa_fcport_is_trunk_enabled(struct bfa_s *bfa)
         return fcport->cfg.trunked;
 }
 
+void
+bfa_fcport_dportenable(struct bfa_s *bfa)
+{
+        /*
+         * Assume caller check for port is in disable state
+         */
+        bfa_sm_send_event(BFA_FCPORT_MOD(bfa), BFA_FCPORT_SM_DPORTENABLE);
+        bfa_port_set_dportenabled(&bfa->modules.port, BFA_TRUE);
+}
+
+void
+bfa_fcport_dportdisable(struct bfa_s *bfa)
+{
+        /*
+         * Assume caller check for port is in disable state
+         */
+        bfa_sm_send_event(BFA_FCPORT_MOD(bfa), BFA_FCPORT_SM_DPORTDISABLE);
+        bfa_port_set_dportenabled(&bfa->modules.port, BFA_FALSE);
+}
+
 /*
  * Rport State machine functions
  */
@@ -4707,6 +4963,21 @@ bfa_rport_isr(struct bfa_s *bfa, struct bfi_msg_s *m)
                 bfa_sm_send_event(rp, BFA_RPORT_SM_QOS_SCN);
                 break;
 
+        case BFI_RPORT_I2H_LIP_SCN_ONLINE:
+                bfa_fcport_update_loop_info(BFA_FCPORT_MOD(bfa),
+                                &msg.lip_scn->loop_info);
+                bfa_cb_rport_scn_online(bfa);
+                break;
+
+        case BFI_RPORT_I2H_LIP_SCN_OFFLINE:
+                bfa_cb_rport_scn_offline(bfa);
+                break;
+
+        case BFI_RPORT_I2H_NO_DEV:
+                rp = BFA_RPORT_FROM_TAG(bfa, msg.lip_scn->bfa_handle);
+                bfa_cb_rport_scn_no_dev(rp->rport_drv);
+                break;
+
         default:
                 bfa_trc(bfa, m->mhdr.msg_id);
                 WARN_ON(1);
@@ -5348,6 +5619,37 @@ bfa_uf_res_recfg(struct bfa_s *bfa, u16 num_uf_fw)
 }
 
 /*
+ *      Dport forward declaration
+ */
+
+/*
+ * BFA DPORT state machine events
+ */
+enum bfa_dport_sm_event {
+        BFA_DPORT_SM_ENABLE     = 1,    /* dport enable event         */
+        BFA_DPORT_SM_DISABLE    = 2,    /* dport disable event        */
+        BFA_DPORT_SM_FWRSP      = 3,    /* fw enable/disable rsp      */
+        BFA_DPORT_SM_QRESUME    = 4,    /* CQ space available         */
+        BFA_DPORT_SM_HWFAIL     = 5,    /* IOC h/w failure            */
+};
+
+static void bfa_dport_sm_disabled(struct bfa_dport_s *dport,
+                                  enum bfa_dport_sm_event event);
+static void bfa_dport_sm_enabling_qwait(struct bfa_dport_s *dport,
+                                  enum bfa_dport_sm_event event);
+static void bfa_dport_sm_enabling(struct bfa_dport_s *dport,
+                                  enum bfa_dport_sm_event event);
+static void bfa_dport_sm_enabled(struct bfa_dport_s *dport,
+                                 enum bfa_dport_sm_event event);
+static void bfa_dport_sm_disabling_qwait(struct bfa_dport_s *dport,
+                                 enum bfa_dport_sm_event event);
+static void bfa_dport_sm_disabling(struct bfa_dport_s *dport,
+                                   enum bfa_dport_sm_event event);
+static void bfa_dport_qresume(void *cbarg);
+static void bfa_dport_req_comp(struct bfa_dport_s *dport,
+                               bfi_diag_dport_rsp_t *msg);
+
+/*
  *      BFA fcdiag module
  */
 #define BFA_DIAG_QTEST_TOV      1000    /* msec */
@@ -5377,15 +5679,24 @@ bfa_fcdiag_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
                 struct bfa_pcidev_s *pcidev)
 {
         struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+        struct bfa_dport_s  *dport = &fcdiag->dport;
+
         fcdiag->bfa             = bfa;
         fcdiag->trcmod  = bfa->trcmod;
         /* The common DIAG attach bfa_diag_attach() will do all memory claim */
+        dport->bfa = bfa;
+        bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+        bfa_reqq_winit(&dport->reqq_wait, bfa_dport_qresume, dport);
+        dport->cbfn = NULL;
+        dport->cbarg = NULL;
 }
 
 static void
 bfa_fcdiag_iocdisable(struct bfa_s *bfa)
 {
         struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+        struct bfa_dport_s *dport = &fcdiag->dport;
+
         bfa_trc(fcdiag, fcdiag->lb.lock);
         if (fcdiag->lb.lock) {
                 fcdiag->lb.status = BFA_STATUS_IOC_FAILURE;
@@ -5393,6 +5704,8 @@ bfa_fcdiag_iocdisable(struct bfa_s *bfa)
                 fcdiag->lb.lock = 0;
                 bfa_fcdiag_set_busy_status(fcdiag);
         }
+
+        bfa_sm_send_event(dport, BFA_DPORT_SM_HWFAIL);
 }
 
 static void
@@ -5577,6 +5890,9 @@ bfa_fcdiag_intr(struct bfa_s *bfa, struct bfi_msg_s *msg)
         case BFI_DIAG_I2H_QTEST:
                 bfa_fcdiag_queuetest_comp(fcdiag, (bfi_diag_qtest_rsp_t *)msg);
                 break;
+        case BFI_DIAG_I2H_DPORT:
+                bfa_dport_req_comp(&fcdiag->dport, (bfi_diag_dport_rsp_t *)msg);
+                break;
         default:
                 bfa_trc(fcdiag, msg->mhdr.msg_id);
                 WARN_ON(1);
@@ -5646,12 +5962,18 @@ bfa_fcdiag_loopback(struct bfa_s *bfa, enum bfa_port_opmode opmode,
                 }
         }
 
+        /*
+         * For CT2, 1G is not supported
+         */
+        if ((speed == BFA_PORT_SPEED_1GBPS) &&
+            (bfa_asic_id_ct2(bfa->ioc.pcidev.device_id))) {
+                bfa_trc(fcdiag, speed);
+                return BFA_STATUS_UNSUPP_SPEED;
+        }
+
         /* For Mezz card, port speed entered needs to be checked */
         if (bfa_mfg_is_mezz(bfa->ioc.attr->card_type)) {
                 if (bfa_ioc_get_type(&bfa->ioc) == BFA_IOC_TYPE_FC) {
-                        if ((speed == BFA_PORT_SPEED_1GBPS) &&
-                            (bfa_asic_id_ct2(bfa->ioc.pcidev.device_id)))
-                                return BFA_STATUS_UNSUPP_SPEED;
                         if (!(speed == BFA_PORT_SPEED_1GBPS ||
                               speed == BFA_PORT_SPEED_2GBPS ||
                               speed == BFA_PORT_SPEED_4GBPS ||
@@ -5764,3 +6086,379 @@ bfa_fcdiag_lb_is_running(struct bfa_s *bfa)
         struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
         return fcdiag->lb.lock ?  BFA_STATUS_DIAG_BUSY : BFA_STATUS_OK;
 }
+
+/*
+ *      D-port
+ */
+static bfa_boolean_t bfa_dport_send_req(struct bfa_dport_s *dport,
+                                        enum bfi_dport_req req);
+static void
+bfa_cb_fcdiag_dport(struct bfa_dport_s *dport, bfa_status_t bfa_status)
+{
+        if (dport->cbfn != NULL) {
+                dport->cbfn(dport->cbarg, bfa_status);
+                dport->cbfn = NULL;
+                dport->cbarg = NULL;
+        }
+}
+
+static void
+bfa_dport_sm_disabled(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+        bfa_trc(dport->bfa, event);
+
+        switch (event) {
+        case BFA_DPORT_SM_ENABLE:
+                bfa_fcport_dportenable(dport->bfa);
+                if (bfa_dport_send_req(dport, BFI_DPORT_ENABLE))
+                        bfa_sm_set_state(dport, bfa_dport_sm_enabling);
+                else
+                        bfa_sm_set_state(dport, bfa_dport_sm_enabling_qwait);
+                break;
+
+        case BFA_DPORT_SM_DISABLE:
+                /* Already disabled */
+                break;
+
+        case BFA_DPORT_SM_HWFAIL:
+                /* ignore */
+                break;
+
+        default:
+                bfa_sm_fault(dport->bfa, event);
+        }
+}
+
+static void
+bfa_dport_sm_enabling_qwait(struct bfa_dport_s *dport,
+                            enum bfa_dport_sm_event event)
+{
+        bfa_trc(dport->bfa, event);
+
+        switch (event) {
+        case BFA_DPORT_SM_QRESUME:
+                bfa_sm_set_state(dport, bfa_dport_sm_enabling);
+                bfa_dport_send_req(dport, BFI_DPORT_ENABLE);
+                break;
+
+        case BFA_DPORT_SM_HWFAIL:
+                bfa_reqq_wcancel(&dport->reqq_wait);
+                bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+                bfa_cb_fcdiag_dport(dport, BFA_STATUS_FAILED);
+                break;
+
+        default:
+                bfa_sm_fault(dport->bfa, event);
+        }
+}
+
+static void
+bfa_dport_sm_enabling(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+        bfa_trc(dport->bfa, event);
+
+        switch (event) {
+        case BFA_DPORT_SM_FWRSP:
+                bfa_sm_set_state(dport, bfa_dport_sm_enabled);
+                break;
+
+        case BFA_DPORT_SM_HWFAIL:
+                bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+                bfa_cb_fcdiag_dport(dport, BFA_STATUS_FAILED);
+                break;
+
+        default:
+                bfa_sm_fault(dport->bfa, event);
+        }
+}
+
+static void
+bfa_dport_sm_enabled(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+        bfa_trc(dport->bfa, event);
+
+        switch (event) {
+        case BFA_DPORT_SM_ENABLE:
+                /* Already enabled */
+                break;
+
+        case BFA_DPORT_SM_DISABLE:
+                bfa_fcport_dportdisable(dport->bfa);
+                if (bfa_dport_send_req(dport, BFI_DPORT_DISABLE))
+                        bfa_sm_set_state(dport, bfa_dport_sm_disabling);
+                else
+                        bfa_sm_set_state(dport, bfa_dport_sm_disabling_qwait);
+                break;
+
+        case BFA_DPORT_SM_HWFAIL:
+                bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+                break;
+
+        default:
+                bfa_sm_fault(dport->bfa, event);
+        }
+}
+
+static void
+bfa_dport_sm_disabling_qwait(struct bfa_dport_s *dport,
+                             enum bfa_dport_sm_event event)
+{
+        bfa_trc(dport->bfa, event);
+
+        switch (event) {
+        case BFA_DPORT_SM_QRESUME:
+                bfa_sm_set_state(dport, bfa_dport_sm_disabling);
+                bfa_dport_send_req(dport, BFI_DPORT_DISABLE);
+                break;
+
+        case BFA_DPORT_SM_HWFAIL:
+                bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+                bfa_reqq_wcancel(&dport->reqq_wait);
+                bfa_cb_fcdiag_dport(dport, BFA_STATUS_OK);
+                break;
+
+        default:
+                bfa_sm_fault(dport->bfa, event);
+        }
+}
+
+static void
+bfa_dport_sm_disabling(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+        bfa_trc(dport->bfa, event);
+
+        switch (event) {
+        case BFA_DPORT_SM_FWRSP:
+                bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+                break;
+
+        case BFA_DPORT_SM_HWFAIL:
+                bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+                bfa_cb_fcdiag_dport(dport, BFA_STATUS_OK);
+                break;
+
+        default:
+                bfa_sm_fault(dport->bfa, event);
+        }
+}
+
+
+static bfa_boolean_t
+bfa_dport_send_req(struct bfa_dport_s *dport, enum bfi_dport_req req)
+{
+        struct bfi_diag_dport_req_s *m;
+
+        /*
+         * Increment message tag before queue check, so that responses to old
+         * requests are discarded.
+         */
+        dport->msgtag++;
+
+        /*
+         * check for room in queue to send request now
+         */
+        m = bfa_reqq_next(dport->bfa, BFA_REQQ_DIAG);
+        if (!m) {
+                bfa_reqq_wait(dport->bfa, BFA_REQQ_PORT, &dport->reqq_wait);
+                return BFA_FALSE;
+        }
+
+        bfi_h2i_set(m->mh, BFI_MC_DIAG, BFI_DIAG_H2I_DPORT,
+                    bfa_fn_lpu(dport->bfa));
+        m->req  = req;
+        m->msgtag = dport->msgtag;
+
+        /*
+         * queue I/O message to firmware
+         */
+        bfa_reqq_produce(dport->bfa, BFA_REQQ_DIAG, m->mh);
+
+        return BFA_TRUE;
+}
+
+static void
+bfa_dport_qresume(void *cbarg)
+{
+        struct bfa_dport_s *dport = cbarg;
+
+        bfa_sm_send_event(dport, BFA_DPORT_SM_QRESUME);
+}
+
+static void
+bfa_dport_req_comp(struct bfa_dport_s *dport, bfi_diag_dport_rsp_t *msg)
+{
+        bfa_sm_send_event(dport, BFA_DPORT_SM_FWRSP);
+        bfa_cb_fcdiag_dport(dport, msg->status);
+}
+
+/*
+ * Dport enable
+ *
+ * @param[in] *bfa            - bfa data struct
+ */
+bfa_status_t
+bfa_dport_enable(struct bfa_s *bfa, bfa_cb_diag_t cbfn, void *cbarg)
+{
+        struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+        struct bfa_dport_s  *dport = &fcdiag->dport;
+
+        /*
+         * Dport is not support in MEZZ card
+         */
+        if (bfa_mfg_is_mezz(dport->bfa->ioc.attr->card_type)) {
+                bfa_trc(dport->bfa, BFA_STATUS_PBC);
+                return BFA_STATUS_CMD_NOTSUPP_MEZZ;
+        }
+
+        /*
+         * Check to see if IOC is down
+        */
+        if (!bfa_iocfc_is_operational(bfa))
+                return BFA_STATUS_IOC_NON_OP;
+
+        /* if port is PBC disabled, return error */
+        if (bfa_fcport_is_pbcdisabled(bfa)) {
+                bfa_trc(dport->bfa, BFA_STATUS_PBC);
+                return BFA_STATUS_PBC;
+        }
+
+        /*
+         * Check if port mode is FC port
+         */
+        if (bfa_ioc_get_type(&bfa->ioc) != BFA_IOC_TYPE_FC) {
+                bfa_trc(dport->bfa, bfa_ioc_get_type(&bfa->ioc));
+                return BFA_STATUS_CMD_NOTSUPP_CNA;
+        }
+
+        /*
+         * Check if port is in LOOP mode
+         */
+        if ((bfa_fcport_get_cfg_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP) ||
+            (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP)) {
+                bfa_trc(dport->bfa, 0);
+                return BFA_STATUS_TOPOLOGY_LOOP;
+        }
+
+        /*
+         * Check if port is TRUNK mode
+         */
+        if (bfa_fcport_is_trunk_enabled(bfa)) {
+                bfa_trc(dport->bfa, 0);
+                return BFA_STATUS_ERROR_TRUNK_ENABLED;
+        }
+
+        /*
+         * Check to see if port is disable or in dport state
+         */
+        if ((bfa_fcport_is_disabled(bfa) == BFA_FALSE) &&
+            (bfa_fcport_is_dport(bfa) == BFA_FALSE)) {
+                bfa_trc(dport->bfa, 0);
+                return BFA_STATUS_PORT_NOT_DISABLED;
+        }
+
+        /*
+         * Check if dport is busy
+         */
+        if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabling) ||
+            bfa_sm_cmp_state(dport, bfa_dport_sm_enabling_qwait) ||
+            bfa_sm_cmp_state(dport, bfa_dport_sm_disabling) ||
+            bfa_sm_cmp_state(dport, bfa_dport_sm_disabling_qwait)) {
+                return BFA_STATUS_DEVBUSY;
+        }
+
+        /*
+         * Check if dport is already enabled
+         */
+        if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabled)) {
+                bfa_trc(dport->bfa, 0);
+                return BFA_STATUS_DPORT_ENABLED;
+        }
+
+        dport->cbfn = cbfn;
+        dport->cbarg = cbarg;
+
+        bfa_sm_send_event(dport, BFA_DPORT_SM_ENABLE);
+        return BFA_STATUS_OK;
+}
+
+/*
+ *      Dport disable
+ *
+ *      @param[in] *bfa            - bfa data struct
+ */
+bfa_status_t
+bfa_dport_disable(struct bfa_s *bfa, bfa_cb_diag_t cbfn, void *cbarg)
+{
+        struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+        struct bfa_dport_s *dport = &fcdiag->dport;
+
+        if (bfa_ioc_is_disabled(&bfa->ioc))
+                return BFA_STATUS_IOC_DISABLED;
+
+        /* if port is PBC disabled, return error */
+        if (bfa_fcport_is_pbcdisabled(bfa)) {
+                bfa_trc(dport->bfa, BFA_STATUS_PBC);
+                return BFA_STATUS_PBC;
+        }
+
+        /*
+         * Check to see if port is disable or in dport state
+         */
+        if ((bfa_fcport_is_disabled(bfa) == BFA_FALSE) &&
+            (bfa_fcport_is_dport(bfa) == BFA_FALSE)) {
+                bfa_trc(dport->bfa, 0);
+                return BFA_STATUS_PORT_NOT_DISABLED;
+        }
+
+        /*
+         * Check if dport is busy
+         */
+        if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabling) ||
+            bfa_sm_cmp_state(dport, bfa_dport_sm_enabling_qwait) ||
+            bfa_sm_cmp_state(dport, bfa_dport_sm_disabling) ||
+            bfa_sm_cmp_state(dport, bfa_dport_sm_disabling_qwait))
+                return BFA_STATUS_DEVBUSY;
+
+        /*
+         * Check if dport is already disabled
+         */
+        if (bfa_sm_cmp_state(dport, bfa_dport_sm_disabled)) {
+                bfa_trc(dport->bfa, 0);
+                return BFA_STATUS_DPORT_DISABLED;
+        }
+
+        dport->cbfn = cbfn;
+        dport->cbarg = cbarg;
+
+        bfa_sm_send_event(dport, BFA_DPORT_SM_DISABLE);
+        return BFA_STATUS_OK;
+}
+
+/*
+ *      Get D-port state
+ *
+ * @param[in] *bfa            - bfa data struct
+ */
+
+bfa_status_t
+bfa_dport_get_state(struct bfa_s *bfa, enum bfa_dport_state *state)
+{
+        struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+        struct bfa_dport_s *dport = &fcdiag->dport;
+
+        if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabled))
+                *state = BFA_DPORT_ST_ENABLED;
+        else if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabling) ||
+                 bfa_sm_cmp_state(dport, bfa_dport_sm_enabling_qwait))
+                *state = BFA_DPORT_ST_ENABLING;
+        else if (bfa_sm_cmp_state(dport, bfa_dport_sm_disabled))
+                *state = BFA_DPORT_ST_DISABLED;
+        else if (bfa_sm_cmp_state(dport, bfa_dport_sm_disabling) ||
+                 bfa_sm_cmp_state(dport, bfa_dport_sm_disabling_qwait))
+                *state = BFA_DPORT_ST_DISABLING;
+        else {
+                bfa_trc(dport->bfa, BFA_STATUS_EINVAL);
+                return BFA_STATUS_EINVAL;
+        }
+        return BFA_STATUS_OK;
+}
diff --git a/drivers/scsi/bfa/bfa_svc.h b/drivers/scsi/bfa/bfa_svc.h
index 1abcf7c51661..8d7fbecfcb22 100644
--- a/drivers/scsi/bfa/bfa_svc.h
+++ b/drivers/scsi/bfa/bfa_svc.h
@@ -474,8 +474,10 @@ struct bfa_fcport_s {
         /*  supported speeds */
         enum bfa_port_speed speed;      /*  current speed */
         enum bfa_port_topology topology;        /*  current topology */
-        u8                      myalpa; /*  my ALPA in LOOP topology */
         u8                      rsvd[3];
+        u8                      myalpa; /*  my ALPA in LOOP topology */
+        u8                      alpabm_valid; /* alpa bitmap valid or not */
+        struct fc_alpabm_s      alpabm; /* alpa bitmap */
         struct bfa_port_cfg_s   cfg;    /*  current port configuration */
         bfa_boolean_t           use_flash_cfg; /* get port cfg from flash */
         struct bfa_qos_attr_s  qos_attr;   /* QoS Attributes */
@@ -512,6 +514,7 @@ struct bfa_fcport_s {
         struct bfa_fcport_trunk_s trunk;
         u16             fcoe_vlan;
         struct bfa_mem_dma_s    fcport_dma;
+        bfa_boolean_t           stats_dma_ready;
 };
 
 #define BFA_FCPORT_MOD(__bfa)   (&(__bfa)->modules.fcport)
@@ -534,6 +537,7 @@ enum bfa_port_speed bfa_fcport_get_speed(struct bfa_s *bfa);
 bfa_status_t bfa_fcport_cfg_topology(struct bfa_s *bfa,
                                      enum bfa_port_topology topo);
 enum bfa_port_topology bfa_fcport_get_topology(struct bfa_s *bfa);
+enum bfa_port_topology bfa_fcport_get_cfg_topology(struct bfa_s *bfa);
 bfa_status_t bfa_fcport_cfg_hardalpa(struct bfa_s *bfa, u8 alpa);
 bfa_boolean_t bfa_fcport_get_hardalpa(struct bfa_s *bfa, u8 *alpa);
 u8 bfa_fcport_get_myalpa(struct bfa_s *bfa);
@@ -547,6 +551,9 @@ void bfa_fcport_event_register(struct bfa_s *bfa,
                         void (*event_cbfn) (void *cbarg,
                         enum bfa_port_linkstate event), void *event_cbarg);
 bfa_boolean_t bfa_fcport_is_disabled(struct bfa_s *bfa);
+bfa_boolean_t bfa_fcport_is_dport(struct bfa_s *bfa);
+bfa_status_t bfa_fcport_set_qos_bw(struct bfa_s *bfa,
+                                   struct bfa_qos_bw_s *qos_bw);
 enum bfa_port_speed bfa_fcport_get_ratelim_speed(struct bfa_s *bfa);
 
 void bfa_fcport_set_tx_bbcredit(struct bfa_s *bfa, u16 tx_bbcredit, u8 bb_scn);
@@ -560,6 +567,8 @@ bfa_status_t bfa_fcport_clear_stats(struct bfa_s *bfa,
                         struct bfa_cb_pending_q_s *cb);
 bfa_boolean_t bfa_fcport_is_qos_enabled(struct bfa_s *bfa);
 bfa_boolean_t bfa_fcport_is_trunk_enabled(struct bfa_s *bfa);
+void bfa_fcport_dportenable(struct bfa_s *bfa);
+void bfa_fcport_dportdisable(struct bfa_s *bfa);
 bfa_status_t bfa_fcport_is_pbcdisabled(struct bfa_s *bfa);
 void bfa_fcport_cfg_faa(struct bfa_s *bfa, u8 state);
 
@@ -575,6 +584,9 @@ void bfa_cb_rport_offline(void *rport);
 void bfa_cb_rport_qos_scn_flowid(void *rport,
                                  struct bfa_rport_qos_attr_s old_qos_attr,
                                  struct bfa_rport_qos_attr_s new_qos_attr);
+void bfa_cb_rport_scn_online(struct bfa_s *bfa);
+void bfa_cb_rport_scn_offline(struct bfa_s *bfa);
+void bfa_cb_rport_scn_no_dev(void *rp);
 void bfa_cb_rport_qos_scn_prio(void *rport,
                                struct bfa_rport_qos_attr_s old_qos_attr,
                                struct bfa_rport_qos_attr_s new_qos_attr);
@@ -697,11 +709,21 @@ struct bfa_fcdiag_lb_s {
         u32        status;
 };
 
+struct bfa_dport_s {
+        struct bfa_s    *bfa;           /* Back pointer to BFA  */
+        bfa_sm_t        sm;             /* finite state machine */
+        u32             msgtag;         /* firmware msg tag for reply */
+        struct bfa_reqq_wait_s reqq_wait;
+        bfa_cb_diag_t   cbfn;
+        void            *cbarg;
+};
+
 struct bfa_fcdiag_s {
         struct bfa_s    *bfa;           /* Back pointer to BFA */
         struct bfa_trc_mod_s   *trcmod;
         struct bfa_fcdiag_lb_s lb;
         struct bfa_fcdiag_qtest_s qtest;
+        struct bfa_dport_s      dport;
 };
 
 #define BFA_FCDIAG_MOD(__bfa)   (&(__bfa)->modules.fcdiag)
@@ -717,5 +739,11 @@ bfa_status_t	bfa_fcdiag_queuetest(struct bfa_s *bfa, u32 ignore,
                         u32 queue, struct bfa_diag_qtest_result_s *result,
                         bfa_cb_diag_t cbfn, void *cbarg);
 bfa_status_t    bfa_fcdiag_lb_is_running(struct bfa_s *bfa);
+bfa_status_t    bfa_dport_enable(struct bfa_s *bfa, bfa_cb_diag_t cbfn,
+                                 void *cbarg);
+bfa_status_t    bfa_dport_disable(struct bfa_s *bfa, bfa_cb_diag_t cbfn,
+                                  void *cbarg);
+bfa_status_t    bfa_dport_get_state(struct bfa_s *bfa,
+                                    enum bfa_dport_state *state);
 
 #endif /* __BFA_SVC_H__ */
diff --git a/drivers/scsi/bfa/bfad.c b/drivers/scsi/bfa/bfad.c
index c37494916a1a..895b0e516e07 100644
--- a/drivers/scsi/bfa/bfad.c
+++ b/drivers/scsi/bfa/bfad.c
@@ -63,9 +63,9 @@ int		max_rport_logins = BFA_FCS_MAX_RPORT_LOGINS;
 u32     bfi_image_cb_size, bfi_image_ct_size, bfi_image_ct2_size;
 u32     *bfi_image_cb, *bfi_image_ct, *bfi_image_ct2;
 
-#define BFAD_FW_FILE_CB         "cbfw.bin"
-#define BFAD_FW_FILE_CT         "ctfw.bin"
-#define BFAD_FW_FILE_CT2        "ct2fw.bin"
+#define BFAD_FW_FILE_CB         "cbfw-3.1.0.0.bin"
+#define BFAD_FW_FILE_CT         "ctfw-3.1.0.0.bin"
+#define BFAD_FW_FILE_CT2        "ct2fw-3.1.0.0.bin"
 
 static u32 *bfad_load_fwimg(struct pci_dev *pdev);
 static void bfad_free_fwimg(void);
diff --git a/drivers/scsi/bfa/bfad_bsg.c b/drivers/scsi/bfa/bfad_bsg.c
index 0afa39076cef..555e7db94a1c 100644
--- a/drivers/scsi/bfa/bfad_bsg.c
+++ b/drivers/scsi/bfa/bfad_bsg.c
@@ -33,7 +33,7 @@ bfad_iocmd_ioc_enable(struct bfad_s *bfad, void *cmd)
         /* If IOC is not in disabled state - return */
         if (!bfa_ioc_is_disabled(&bfad->bfa.ioc)) {
                 spin_unlock_irqrestore(&bfad->bfad_lock, flags);
-                iocmd->status = BFA_STATUS_IOC_FAILURE;
+                iocmd->status = BFA_STATUS_OK;
                 return rc;
         }
 
@@ -54,6 +54,12 @@ bfad_iocmd_ioc_disable(struct bfad_s *bfad, void *cmd)
         unsigned long   flags;
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
+        if (bfa_ioc_is_disabled(&bfad->bfa.ioc)) {
+                spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+                iocmd->status = BFA_STATUS_OK;
+                return rc;
+        }
+
         if (bfad->disable_active) {
                 spin_unlock_irqrestore(&bfad->bfad_lock, flags);
                 return -EBUSY;
@@ -101,9 +107,10 @@ bfad_iocmd_ioc_get_info(struct bfad_s *bfad, void *cmd)
 
         /* set adapter hw path */
         strcpy(iocmd->adapter_hwpath, bfad->pci_name);
-        i = strlen(iocmd->adapter_hwpath) - 1;
-        while (iocmd->adapter_hwpath[i] != '.')
-                i--;
+        for (i = 0; iocmd->adapter_hwpath[i] != ':' && i < BFA_STRING_32; i++)
+                ;
+        for (; iocmd->adapter_hwpath[++i] != ':' && i < BFA_STRING_32; )
+                ;
         iocmd->adapter_hwpath[i] = '\0';
         iocmd->status = BFA_STATUS_OK;
         return 0;
@@ -880,6 +887,19 @@ out:
 }
 
 int
+bfad_iocmd_qos_set_bw(struct bfad_s *bfad, void *pcmd)
+{
+        struct bfa_bsg_qos_bw_s *iocmd = (struct bfa_bsg_qos_bw_s *)pcmd;
+        unsigned long   flags;
+
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_fcport_set_qos_bw(&bfad->bfa, &iocmd->qos_bw);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+        return 0;
+}
+
+int
 bfad_iocmd_ratelim(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
 {
         struct bfa_bsg_gen_s *iocmd = (struct bfa_bsg_gen_s *)pcmd;
@@ -888,16 +908,22 @@ bfad_iocmd_ratelim(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
 
-        if (cmd == IOCMD_RATELIM_ENABLE)
-                fcport->cfg.ratelimit = BFA_TRUE;
-        else if (cmd == IOCMD_RATELIM_DISABLE)
-                fcport->cfg.ratelimit = BFA_FALSE;
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else {
+                if (cmd == IOCMD_RATELIM_ENABLE)
+                        fcport->cfg.ratelimit = BFA_TRUE;
+                else if (cmd == IOCMD_RATELIM_DISABLE)
+                        fcport->cfg.ratelimit = BFA_FALSE;
 
-        if (fcport->cfg.trl_def_speed == BFA_PORT_SPEED_UNKNOWN)
-                fcport->cfg.trl_def_speed = BFA_PORT_SPEED_1GBPS;
+                if (fcport->cfg.trl_def_speed == BFA_PORT_SPEED_UNKNOWN)
+                        fcport->cfg.trl_def_speed = BFA_PORT_SPEED_1GBPS;
+
+                iocmd->status = BFA_STATUS_OK;
+        }
 
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
-        iocmd->status = BFA_STATUS_OK;
 
         return 0;
 }
@@ -919,8 +945,13 @@ bfad_iocmd_ratelim_speed(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
                 return 0;
         }
 
-        fcport->cfg.trl_def_speed = iocmd->speed;
-        iocmd->status = BFA_STATUS_OK;
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else {
+                fcport->cfg.trl_def_speed = iocmd->speed;
+                iocmd->status = BFA_STATUS_OK;
+        }
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
         return 0;
@@ -1167,8 +1198,8 @@ bfad_iocmd_pcifn_create(struct bfad_s *bfad, void *cmd)
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         iocmd->status = bfa_ablk_pf_create(&bfad->bfa.modules.ablk,
                                 &iocmd->pcifn_id, iocmd->port,
-                                iocmd->pcifn_class, iocmd->bandwidth,
-                                bfad_hcb_comp, &fcomp);
+                                iocmd->pcifn_class, iocmd->bw_min,
+                                iocmd->bw_max, bfad_hcb_comp, &fcomp);
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
         if (iocmd->status != BFA_STATUS_OK)
                 goto out;
@@ -1211,8 +1242,8 @@ bfad_iocmd_pcifn_bw(struct bfad_s *bfad, void *cmd)
         init_completion(&fcomp.comp);
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         iocmd->status = bfa_ablk_pf_update(&bfad->bfa.modules.ablk,
-                                iocmd->pcifn_id, iocmd->bandwidth,
-                                bfad_hcb_comp, &fcomp);
+                                iocmd->pcifn_id, iocmd->bw_min,
+                                iocmd->bw_max, bfad_hcb_comp, &fcomp);
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
         bfa_trc(bfad, iocmd->status);
         if (iocmd->status != BFA_STATUS_OK)
@@ -1736,6 +1767,52 @@ bfad_iocmd_diag_lb_stat(struct bfad_s *bfad, void *cmd)
 }
 
 int
+bfad_iocmd_diag_cfg_dport(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
+{
+        struct bfa_bsg_gen_s *iocmd = (struct bfa_bsg_gen_s *)pcmd;
+        unsigned long   flags;
+        struct bfad_hal_comp fcomp;
+
+        init_completion(&fcomp.comp);
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        if (cmd == IOCMD_DIAG_DPORT_ENABLE)
+                iocmd->status = bfa_dport_enable(&bfad->bfa,
+                                        bfad_hcb_comp, &fcomp);
+        else if (cmd == IOCMD_DIAG_DPORT_DISABLE)
+                iocmd->status = bfa_dport_disable(&bfad->bfa,
+                                        bfad_hcb_comp, &fcomp);
+        else {
+                bfa_trc(bfad, 0);
+                spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+                return -EINVAL;
+        }
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+        if (iocmd->status != BFA_STATUS_OK)
+                bfa_trc(bfad, iocmd->status);
+        else {
+                wait_for_completion(&fcomp.comp);
+                iocmd->status = fcomp.status;
+        }
+
+        return 0;
+}
+
+int
+bfad_iocmd_diag_dport_get_state(struct bfad_s *bfad, void *pcmd)
+{
+        struct bfa_bsg_diag_dport_get_state_s *iocmd =
+                        (struct bfa_bsg_diag_dport_get_state_s *)pcmd;
+        unsigned long   flags;
+
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_dport_get_state(&bfad->bfa, &iocmd->state);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+        return 0;
+}
+
+int
 bfad_iocmd_phy_get_attr(struct bfad_s *bfad, void *cmd)
 {
         struct bfa_bsg_phy_attr_s *iocmd =
@@ -2052,7 +2129,7 @@ bfad_iocmd_boot_cfg(struct bfad_s *bfad, void *cmd)
         init_completion(&fcomp.comp);
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         iocmd->status = bfa_flash_update_part(BFA_FLASH(&bfad->bfa),
-                        BFA_FLASH_PART_BOOT, PCI_FUNC(bfad->pcidev->devfn),
+                        BFA_FLASH_PART_BOOT, bfad->bfa.ioc.port_id,
                         &iocmd->cfg, sizeof(struct bfa_boot_cfg_s), 0,
                         bfad_hcb_comp, &fcomp);
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
@@ -2074,7 +2151,7 @@ bfad_iocmd_boot_query(struct bfad_s *bfad, void *cmd)
         init_completion(&fcomp.comp);
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         iocmd->status = bfa_flash_read_part(BFA_FLASH(&bfad->bfa),
-                        BFA_FLASH_PART_BOOT, PCI_FUNC(bfad->pcidev->devfn),
+                        BFA_FLASH_PART_BOOT, bfad->bfa.ioc.port_id,
                         &iocmd->cfg, sizeof(struct bfa_boot_cfg_s), 0,
                         bfad_hcb_comp, &fcomp);
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
@@ -2161,22 +2238,31 @@ bfad_iocmd_cfg_trunk(struct bfad_s *bfad, void *cmd, unsigned int v_cmd)
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
 
-        if (v_cmd == IOCMD_TRUNK_ENABLE) {
-                trunk->attr.state = BFA_TRUNK_OFFLINE;
-                bfa_fcport_disable(&bfad->bfa);
-                fcport->cfg.trunked = BFA_TRUE;
-        } else if (v_cmd == IOCMD_TRUNK_DISABLE) {
-                trunk->attr.state = BFA_TRUNK_DISABLED;
-                bfa_fcport_disable(&bfad->bfa);
-                fcport->cfg.trunked = BFA_FALSE;
-        }
+        if (bfa_fcport_is_dport(&bfad->bfa))
+                return BFA_STATUS_DPORT_ERR;
 
-        if (!bfa_fcport_is_disabled(&bfad->bfa))
-                bfa_fcport_enable(&bfad->bfa);
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) ||
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else {
+                if (v_cmd == IOCMD_TRUNK_ENABLE) {
+                        trunk->attr.state = BFA_TRUNK_OFFLINE;
+                        bfa_fcport_disable(&bfad->bfa);
+                        fcport->cfg.trunked = BFA_TRUE;
+                } else if (v_cmd == IOCMD_TRUNK_DISABLE) {
+                        trunk->attr.state = BFA_TRUNK_DISABLED;
+                        bfa_fcport_disable(&bfad->bfa);
+                        fcport->cfg.trunked = BFA_FALSE;
+                }
+
+                if (!bfa_fcport_is_disabled(&bfad->bfa))
+                        bfa_fcport_enable(&bfad->bfa);
+
+                iocmd->status = BFA_STATUS_OK;
+        }
 
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-        iocmd->status = BFA_STATUS_OK;
         return 0;
 }
 
@@ -2189,12 +2275,17 @@ bfad_iocmd_trunk_get_attr(struct bfad_s *bfad, void *cmd)
         unsigned long   flags;
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
-        memcpy((void *)&iocmd->attr, (void *)&trunk->attr,
-                sizeof(struct bfa_trunk_attr_s));
-        iocmd->attr.port_id = bfa_lps_get_base_pid(&bfad->bfa);
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) ||
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else {
+                memcpy((void *)&iocmd->attr, (void *)&trunk->attr,
+                        sizeof(struct bfa_trunk_attr_s));
+                iocmd->attr.port_id = bfa_lps_get_base_pid(&bfad->bfa);
+                iocmd->status = BFA_STATUS_OK;
+        }
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-        iocmd->status = BFA_STATUS_OK;
         return 0;
 }
 
@@ -2207,14 +2298,22 @@ bfad_iocmd_qos(struct bfad_s *bfad, void *cmd, unsigned int v_cmd)
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         if (bfa_ioc_get_type(&bfad->bfa.ioc) == BFA_IOC_TYPE_FC) {
-                if (v_cmd == IOCMD_QOS_ENABLE)
-                        fcport->cfg.qos_enabled = BFA_TRUE;
-                else if (v_cmd == IOCMD_QOS_DISABLE)
-                        fcport->cfg.qos_enabled = BFA_FALSE;
+                if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                        iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+                else {
+                        if (v_cmd == IOCMD_QOS_ENABLE)
+                                fcport->cfg.qos_enabled = BFA_TRUE;
+                        else if (v_cmd == IOCMD_QOS_DISABLE) {
+                                fcport->cfg.qos_enabled = BFA_FALSE;
+                                fcport->cfg.qos_bw.high = BFA_QOS_BW_HIGH;
+                                fcport->cfg.qos_bw.med = BFA_QOS_BW_MED;
+                                fcport->cfg.qos_bw.low = BFA_QOS_BW_LOW;
+                        }
+                }
         }
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-        iocmd->status = BFA_STATUS_OK;
         return 0;
 }
 
@@ -2226,11 +2325,21 @@ bfad_iocmd_qos_get_attr(struct bfad_s *bfad, void *cmd)
         unsigned long   flags;
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
-        iocmd->attr.state = fcport->qos_attr.state;
-        iocmd->attr.total_bb_cr = be32_to_cpu(fcport->qos_attr.total_bb_cr);
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else {
+                iocmd->attr.state = fcport->qos_attr.state;
+                iocmd->attr.total_bb_cr =
+                        be32_to_cpu(fcport->qos_attr.total_bb_cr);
+                iocmd->attr.qos_bw.high = fcport->cfg.qos_bw.high;
+                iocmd->attr.qos_bw.med = fcport->cfg.qos_bw.med;
+                iocmd->attr.qos_bw.low = fcport->cfg.qos_bw.low;
+                iocmd->attr.qos_bw_op = fcport->qos_attr.qos_bw_op;
+                iocmd->status = BFA_STATUS_OK;
+        }
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-        iocmd->status = BFA_STATUS_OK;
         return 0;
 }
 
@@ -2274,6 +2383,7 @@ bfad_iocmd_qos_get_stats(struct bfad_s *bfad, void *cmd)
         struct bfad_hal_comp fcomp;
         unsigned long   flags;
         struct bfa_cb_pending_q_s cb_qe;
+        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(&bfad->bfa);
 
         init_completion(&fcomp.comp);
         bfa_pending_q_init(&cb_qe, (bfa_cb_cbfn_t)bfad_hcb_comp,
@@ -2281,7 +2391,11 @@ bfad_iocmd_qos_get_stats(struct bfad_s *bfad, void *cmd)
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         WARN_ON(!bfa_ioc_get_fcmode(&bfad->bfa.ioc));
-        iocmd->status = bfa_fcport_get_stats(&bfad->bfa, &cb_qe);
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else
+                iocmd->status = bfa_fcport_get_stats(&bfad->bfa, &cb_qe);
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
         if (iocmd->status != BFA_STATUS_OK) {
                 bfa_trc(bfad, iocmd->status);
@@ -2300,6 +2414,7 @@ bfad_iocmd_qos_reset_stats(struct bfad_s *bfad, void *cmd)
         struct bfad_hal_comp fcomp;
         unsigned long   flags;
         struct bfa_cb_pending_q_s cb_qe;
+        struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(&bfad->bfa);
 
         init_completion(&fcomp.comp);
         bfa_pending_q_init(&cb_qe, (bfa_cb_cbfn_t)bfad_hcb_comp,
@@ -2307,7 +2422,11 @@ bfad_iocmd_qos_reset_stats(struct bfad_s *bfad, void *cmd)
 
         spin_lock_irqsave(&bfad->bfad_lock, flags);
         WARN_ON(!bfa_ioc_get_fcmode(&bfad->bfa.ioc));
-        iocmd->status = bfa_fcport_clear_stats(&bfad->bfa, &cb_qe);
+        if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+                (fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+                iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+        else
+                iocmd->status = bfa_fcport_clear_stats(&bfad->bfa, &cb_qe);
         spin_unlock_irqrestore(&bfad->bfad_lock, flags);
         if (iocmd->status != BFA_STATUS_OK) {
                 bfa_trc(bfad, iocmd->status);
@@ -2435,6 +2554,139 @@ bfad_iocmd_fcpim_cfg_lunmask(struct bfad_s *bfad, void *cmd, unsigned int v_cmd)
         return 0;
 }
 
+int
+bfad_iocmd_fcpim_throttle_query(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_fcpim_throttle_s *iocmd =
+                        (struct bfa_bsg_fcpim_throttle_s *)cmd;
+        unsigned long   flags;
+
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_fcpim_throttle_get(&bfad->bfa,
+                                (void *)&iocmd->throttle);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+        return 0;
+}
+
+int
+bfad_iocmd_fcpim_throttle_set(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_fcpim_throttle_s *iocmd =
+                        (struct bfa_bsg_fcpim_throttle_s *)cmd;
+        unsigned long   flags;
+
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_fcpim_throttle_set(&bfad->bfa,
+                                iocmd->throttle.cfg_value);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+        return 0;
+}
+
+int
+bfad_iocmd_tfru_read(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_tfru_s *iocmd =
+                        (struct bfa_bsg_tfru_s *)cmd;
+        struct bfad_hal_comp fcomp;
+        unsigned long flags = 0;
+
+        init_completion(&fcomp.comp);
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_tfru_read(BFA_FRU(&bfad->bfa),
+                                &iocmd->data, iocmd->len, iocmd->offset,
+                                bfad_hcb_comp, &fcomp);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+        if (iocmd->status == BFA_STATUS_OK) {
+                wait_for_completion(&fcomp.comp);
+                iocmd->status = fcomp.status;
+        }
+
+        return 0;
+}
+
+int
+bfad_iocmd_tfru_write(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_tfru_s *iocmd =
+                        (struct bfa_bsg_tfru_s *)cmd;
+        struct bfad_hal_comp fcomp;
+        unsigned long flags = 0;
+
+        init_completion(&fcomp.comp);
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_tfru_write(BFA_FRU(&bfad->bfa),
+                                &iocmd->data, iocmd->len, iocmd->offset,
+                                bfad_hcb_comp, &fcomp);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+        if (iocmd->status == BFA_STATUS_OK) {
+                wait_for_completion(&fcomp.comp);
+                iocmd->status = fcomp.status;
+        }
+
+        return 0;
+}
+
+int
+bfad_iocmd_fruvpd_read(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_fruvpd_s *iocmd =
+                        (struct bfa_bsg_fruvpd_s *)cmd;
+        struct bfad_hal_comp fcomp;
+        unsigned long flags = 0;
+
+        init_completion(&fcomp.comp);
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_fruvpd_read(BFA_FRU(&bfad->bfa),
+                                &iocmd->data, iocmd->len, iocmd->offset,
+                                bfad_hcb_comp, &fcomp);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+        if (iocmd->status == BFA_STATUS_OK) {
+                wait_for_completion(&fcomp.comp);
+                iocmd->status = fcomp.status;
+        }
+
+        return 0;
+}
+
+int
+bfad_iocmd_fruvpd_update(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_fruvpd_s *iocmd =
+                        (struct bfa_bsg_fruvpd_s *)cmd;
+        struct bfad_hal_comp fcomp;
+        unsigned long flags = 0;
+
+        init_completion(&fcomp.comp);
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_fruvpd_update(BFA_FRU(&bfad->bfa),
+                                &iocmd->data, iocmd->len, iocmd->offset,
+                                bfad_hcb_comp, &fcomp);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+        if (iocmd->status == BFA_STATUS_OK) {
+                wait_for_completion(&fcomp.comp);
+                iocmd->status = fcomp.status;
+        }
+
+        return 0;
+}
+
+int
+bfad_iocmd_fruvpd_get_max_size(struct bfad_s *bfad, void *cmd)
+{
+        struct bfa_bsg_fruvpd_max_size_s *iocmd =
+                        (struct bfa_bsg_fruvpd_max_size_s *)cmd;
+        unsigned long flags = 0;
+
+        spin_lock_irqsave(&bfad->bfad_lock, flags);
+        iocmd->status = bfa_fruvpd_get_max_size(BFA_FRU(&bfad->bfa),
+                                                &iocmd->max_size);
+        spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+        return 0;
+}
+
 static int
 bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
                 unsigned int payload_len)
@@ -2660,6 +2912,13 @@ bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
         case IOCMD_DIAG_LB_STAT:
                 rc = bfad_iocmd_diag_lb_stat(bfad, iocmd);
                 break;
+        case IOCMD_DIAG_DPORT_ENABLE:
+        case IOCMD_DIAG_DPORT_DISABLE:
+                rc = bfad_iocmd_diag_cfg_dport(bfad, cmd, iocmd);
+                break;
+        case IOCMD_DIAG_DPORT_GET_STATE:
+                rc = bfad_iocmd_diag_dport_get_state(bfad, iocmd);
+                break;
         case IOCMD_PHY_GET_ATTR:
                 rc = bfad_iocmd_phy_get_attr(bfad, iocmd);
                 break;
@@ -2741,6 +3000,9 @@ bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
         case IOCMD_QOS_RESET_STATS:
                 rc = bfad_iocmd_qos_reset_stats(bfad, iocmd);
                 break;
+        case IOCMD_QOS_SET_BW:
+                rc = bfad_iocmd_qos_set_bw(bfad, iocmd);
+                break;
         case IOCMD_VF_GET_STATS:
                 rc = bfad_iocmd_vf_get_stats(bfad, iocmd);
                 break;
@@ -2759,6 +3021,29 @@ bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
         case IOCMD_FCPIM_LUNMASK_DELETE:
                 rc = bfad_iocmd_fcpim_cfg_lunmask(bfad, iocmd, cmd);
                 break;
+        case IOCMD_FCPIM_THROTTLE_QUERY:
+                rc = bfad_iocmd_fcpim_throttle_query(bfad, iocmd);
+                break;
+        case IOCMD_FCPIM_THROTTLE_SET:
+                rc = bfad_iocmd_fcpim_throttle_set(bfad, iocmd);
+                break;
+        /* TFRU */
+        case IOCMD_TFRU_READ:
+                rc = bfad_iocmd_tfru_read(bfad, iocmd);
+                break;
+        case IOCMD_TFRU_WRITE:
+                rc = bfad_iocmd_tfru_write(bfad, iocmd);
+                break;
+        /* FRU */
+        case IOCMD_FRUVPD_READ:
+                rc = bfad_iocmd_fruvpd_read(bfad, iocmd);
+                break;
+        case IOCMD_FRUVPD_UPDATE:
+                rc = bfad_iocmd_fruvpd_update(bfad, iocmd);
+                break;
+        case IOCMD_FRUVPD_GET_MAX_SIZE:
+                rc = bfad_iocmd_fruvpd_get_max_size(bfad, iocmd);
+                break;
         default:
                 rc = -EINVAL;
                 break;
diff --git a/drivers/scsi/bfa/bfad_bsg.h b/drivers/scsi/bfa/bfad_bsg.h
index 8c569ddb750d..15e1fc8e796b 100644
--- a/drivers/scsi/bfa/bfad_bsg.h
+++ b/drivers/scsi/bfa/bfad_bsg.h
@@ -141,6 +141,17 @@ enum {
         IOCMD_FCPIM_LUNMASK_QUERY,
         IOCMD_FCPIM_LUNMASK_ADD,
         IOCMD_FCPIM_LUNMASK_DELETE,
+        IOCMD_DIAG_DPORT_ENABLE,
+        IOCMD_DIAG_DPORT_DISABLE,
+        IOCMD_DIAG_DPORT_GET_STATE,
+        IOCMD_QOS_SET_BW,
+        IOCMD_FCPIM_THROTTLE_QUERY,
+        IOCMD_FCPIM_THROTTLE_SET,
+        IOCMD_TFRU_READ,
+        IOCMD_TFRU_WRITE,
+        IOCMD_FRUVPD_READ,
+        IOCMD_FRUVPD_UPDATE,
+        IOCMD_FRUVPD_GET_MAX_SIZE,
 };
 
 struct bfa_bsg_gen_s {
@@ -463,7 +474,8 @@ struct bfa_bsg_pcifn_s {
         bfa_status_t            status;
         u16                     bfad_num;
         u16                     pcifn_id;
-        u32                     bandwidth;
+        u16                     bw_min;
+        u16                     bw_max;
         u8                      port;
         enum bfi_pcifn_class    pcifn_class;
         u8                      rsvd[1];
@@ -613,6 +625,13 @@ struct bfa_bsg_diag_lb_stat_s {
         u16             rsvd;
 };
 
+struct bfa_bsg_diag_dport_get_state_s {
+        bfa_status_t    status;
+        u16             bfad_num;
+        u16             rsvd;
+        enum bfa_dport_state state;
+};
+
 struct bfa_bsg_phy_attr_s {
         bfa_status_t    status;
         u16     bfad_num;
@@ -694,6 +713,13 @@ struct bfa_bsg_qos_vc_attr_s {
         struct bfa_qos_vc_attr_s attr;
 };
 
+struct bfa_bsg_qos_bw_s {
+        bfa_status_t    status;
+        u16             bfad_num;
+        u16             rsvd;
+        struct bfa_qos_bw_s qos_bw;
+};
+
 struct bfa_bsg_vf_stats_s {
         bfa_status_t    status;
         u16             bfad_num;
@@ -722,6 +748,41 @@ struct bfa_bsg_fcpim_lunmask_s {
         struct scsi_lun lun;
 };
 
+struct bfa_bsg_fcpim_throttle_s {
+        bfa_status_t    status;
+        u16             bfad_num;
+        u16             vf_id;
+        struct bfa_defs_fcpim_throttle_s throttle;
+};
+
+#define BFA_TFRU_DATA_SIZE              64
+#define BFA_MAX_FRUVPD_TRANSFER_SIZE    0x1000
+
+struct bfa_bsg_tfru_s {
+        bfa_status_t    status;
+        u16             bfad_num;
+        u16             rsvd;
+        u32             offset;
+        u32             len;
+        u8              data[BFA_TFRU_DATA_SIZE];
+};
+
+struct bfa_bsg_fruvpd_s {
+        bfa_status_t    status;
+        u16             bfad_num;
+        u16             rsvd;
+        u32             offset;
+        u32             len;
+        u8              data[BFA_MAX_FRUVPD_TRANSFER_SIZE];
+};
+
+struct bfa_bsg_fruvpd_max_size_s {
+        bfa_status_t    status;
+        u16             bfad_num;
+        u16             rsvd;
+        u32             max_size;
+};
+
 struct bfa_bsg_fcpt_s {
         bfa_status_t    status;
         u16             vf_id;
diff --git a/drivers/scsi/bfa/bfad_drv.h b/drivers/scsi/bfa/bfad_drv.h
index 1840651ce1d4..0c64a04f01fa 100644
--- a/drivers/scsi/bfa/bfad_drv.h
+++ b/drivers/scsi/bfa/bfad_drv.h
@@ -57,7 +57,7 @@
 #ifdef BFA_DRIVER_VERSION
 #define BFAD_DRIVER_VERSION    BFA_DRIVER_VERSION
 #else
-#define BFAD_DRIVER_VERSION    "3.1.2.0"
+#define BFAD_DRIVER_VERSION    "3.1.2.1"
 #endif
 
 #define BFAD_PROTO_NAME FCPI_NAME
diff --git a/drivers/scsi/bfa/bfi.h b/drivers/scsi/bfa/bfi.h
index b2ba0b2e91b2..57b146bca18c 100644
--- a/drivers/scsi/bfa/bfi.h
+++ b/drivers/scsi/bfa/bfi.h
@@ -210,7 +210,8 @@ enum bfi_mclass {
         BFI_MC_PORT             = 21,   /*  Physical port                   */
         BFI_MC_SFP              = 22,   /*  SFP module  */
         BFI_MC_PHY              = 25,   /*  External PHY message class  */
-        BFI_MC_MAX              = 32
+        BFI_MC_FRU              = 34,
+        BFI_MC_MAX              = 35
 };
 
 #define BFI_IOC_MAX_CQS         4
@@ -288,6 +289,9 @@ struct bfi_ioc_attr_s {
         char            optrom_version[BFA_VERSION_LEN];
         struct          bfa_mfg_vpd_s   vpd;
         u32     card_type;      /*  card type                   */
+        u8      mfg_day;        /* manufacturing day */
+        u8      mfg_month;      /* manufacturing month */
+        u16     mfg_year;       /* manufacturing year */
 };
 
 /*
@@ -687,7 +691,8 @@ struct bfi_ablk_h2i_pf_req_s {
         u8                      pcifn;
         u8                      port;
         u16                     pers;
-        u32                     bw;
+        u16                     bw_min; /* percent BW @ max speed */
+        u16                     bw_max; /* percent BW @ max speed */
 };
 
 /* BFI_ABLK_H2I_OPTROM_ENABLE, BFI_ABLK_H2I_OPTROM_DISABLE */
@@ -957,6 +962,7 @@ enum bfi_diag_h2i {
         BFI_DIAG_H2I_TEMPSENSOR = 4,
         BFI_DIAG_H2I_LEDTEST = 5,
         BFI_DIAG_H2I_QTEST      = 6,
+        BFI_DIAG_H2I_DPORT      = 7,
 };
 
 enum bfi_diag_i2h {
@@ -966,6 +972,7 @@ enum bfi_diag_i2h {
         BFI_DIAG_I2H_TEMPSENSOR = BFA_I2HM(BFI_DIAG_H2I_TEMPSENSOR),
         BFI_DIAG_I2H_LEDTEST = BFA_I2HM(BFI_DIAG_H2I_LEDTEST),
         BFI_DIAG_I2H_QTEST      = BFA_I2HM(BFI_DIAG_H2I_QTEST),
+        BFI_DIAG_I2H_DPORT      = BFA_I2HM(BFI_DIAG_H2I_DPORT),
 };
 
 #define BFI_DIAG_MAX_SGES       2
@@ -1052,6 +1059,23 @@ struct bfi_diag_qtest_req_s {
 #define bfi_diag_qtest_rsp_t struct bfi_diag_qtest_req_s
 
 /*
+ *      D-port test
+ */
+enum bfi_dport_req {
+        BFI_DPORT_DISABLE       = 0,    /* disable dport request        */
+        BFI_DPORT_ENABLE        = 1,    /* enable dport request         */
+};
+
+struct bfi_diag_dport_req_s {
+        struct bfi_mhdr_s       mh;     /* 4 bytes                      */
+        u8                      req;    /* request 1: enable 0: disable */
+        u8                      status; /* reply status                 */
+        u8                      rsvd[2];
+        u32                     msgtag; /* msgtag for reply             */
+};
+#define bfi_diag_dport_rsp_t struct bfi_diag_dport_req_s
+
+/*
  *      PHY module specific
  */
 enum bfi_phy_h2i_msgs_e {
@@ -1147,6 +1171,50 @@ struct bfi_phy_write_rsp_s {
         u32                     length;
 };
 
+enum bfi_fru_h2i_msgs {
+        BFI_FRUVPD_H2I_WRITE_REQ = 1,
+        BFI_FRUVPD_H2I_READ_REQ = 2,
+        BFI_TFRU_H2I_WRITE_REQ = 3,
+        BFI_TFRU_H2I_READ_REQ = 4,
+};
+
+enum bfi_fru_i2h_msgs {
+        BFI_FRUVPD_I2H_WRITE_RSP = BFA_I2HM(1),
+        BFI_FRUVPD_I2H_READ_RSP = BFA_I2HM(2),
+        BFI_TFRU_I2H_WRITE_RSP = BFA_I2HM(3),
+        BFI_TFRU_I2H_READ_RSP = BFA_I2HM(4),
+};
+
+/*
+ * FRU write request
+ */
+struct bfi_fru_write_req_s {
+        struct bfi_mhdr_s       mh;     /* Common msg header */
+        u8                      last;
+        u8                      rsv[3];
+        u32                     offset;
+        u32                     length;
+        struct bfi_alen_s       alen;
+};
+
+/*
+ * FRU read request
+ */
+struct bfi_fru_read_req_s {
+        struct bfi_mhdr_s       mh;     /* Common msg header */
+        u32                     offset;
+        u32                     length;
+        struct bfi_alen_s       alen;
+};
+
+/*
+ * FRU response
+ */
+struct bfi_fru_rsp_s {
+        struct bfi_mhdr_s       mh;     /* Common msg header */
+        u32                     status;
+        u32                     length;
+};
 #pragma pack()
 
 #endif /* __BFI_H__ */
diff --git a/drivers/scsi/bfa/bfi_ms.h b/drivers/scsi/bfa/bfi_ms.h
index d4220e13cafa..5ae2c167b2c8 100644
--- a/drivers/scsi/bfa/bfi_ms.h
+++ b/drivers/scsi/bfa/bfi_ms.h
@@ -426,6 +426,7 @@ struct bfi_lps_login_req_s {
         u8              auth_en;
         u8              lps_role;
         u8              bb_scn;
+        u32             vvl_flag;
 };
 
 struct bfi_lps_login_rsp_s {
@@ -499,6 +500,9 @@ enum bfi_rport_i2h_msgs {
         BFI_RPORT_I2H_CREATE_RSP = BFA_I2HM(1),
         BFI_RPORT_I2H_DELETE_RSP = BFA_I2HM(2),
         BFI_RPORT_I2H_QOS_SCN    = BFA_I2HM(3),
+        BFI_RPORT_I2H_LIP_SCN_ONLINE =  BFA_I2HM(4),
+        BFI_RPORT_I2H_LIP_SCN_OFFLINE = BFA_I2HM(5),
+        BFI_RPORT_I2H_NO_DEV    = BFA_I2HM(6),
 };
 
 struct bfi_rport_create_req_s {
@@ -551,6 +555,14 @@ struct bfi_rport_qos_scn_s {
         struct bfa_rport_qos_attr_s new_qos_attr;  /* New QoS Attributes */
 };
 
+struct bfi_rport_lip_scn_s {
+        struct bfi_mhdr_s  mh;          /*!< common msg header  */
+        u16     bfa_handle;     /*!< host rport handle  */
+        u8              status;         /*!< scn online status  */
+        u8              rsvd;
+        struct bfa_fcport_loop_info_s   loop_info;
+};
+
 union bfi_rport_h2i_msg_u {
         struct bfi_msg_s                *msg;
         struct bfi_rport_create_req_s   *create_req;
@@ -563,6 +575,7 @@ union bfi_rport_i2h_msg_u {
         struct bfi_rport_create_rsp_s   *create_rsp;
         struct bfi_rport_delete_rsp_s   *delete_rsp;
         struct bfi_rport_qos_scn_s      *qos_scn_evt;
+        struct bfi_rport_lip_scn_s      *lip_scn;
 };
 
 /*
@@ -828,6 +841,7 @@ enum bfi_tskim_status {
          */
         BFI_TSKIM_STS_TIMEOUT  = 10,    /*  TM request timedout */
         BFI_TSKIM_STS_ABORTED  = 11,    /*  Aborted on host request */
+        BFI_TSKIM_STS_UTAG     = 12,    /*  unknown tag for request */
 };
 
 struct bfi_tskim_rsp_s {
diff --git a/drivers/scsi/bfa/bfi_reg.h b/drivers/scsi/bfa/bfi_reg.h
index ed5f159e1867..99133bcf53f9 100644
--- a/drivers/scsi/bfa/bfi_reg.h
+++ b/drivers/scsi/bfa/bfi_reg.h
@@ -338,6 +338,7 @@ enum {
 #define __A2T_AHB_LOAD                  0x00000800
 #define __WGN_READY                     0x00000400
 #define __GLBL_PF_VF_CFG_RDY            0x00000200
+#define CT2_NFC_STS_REG                 0x00027410
 #define CT2_NFC_CSR_CLR_REG             0x00027420
 #define CT2_NFC_CSR_SET_REG             0x00027424
 #define __HALT_NFC_CONTROLLER           0x00000002
@@ -355,6 +356,8 @@ enum {
         (CT2_CSI_MAC0_CONTROL_REG +     \
         (__n) * (CT2_CSI_MAC1_CONTROL_REG - CT2_CSI_MAC0_CONTROL_REG))
 
+#define CT2_NFC_FLASH_STS_REG           0x00014834
+#define __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS  0x00000020
 /*
  * Name semaphore registers based on usage
  */
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 078d262ac7cc..666b7ac4475f 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1643,7 +1643,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
         skb_reset_network_header(skb);
         skb->mac_len = elen;
         skb->protocol = htons(ETH_P_FCOE);
-        skb->priority = port->priority;
+        skb->priority = fcoe->priority;
 
         if (fcoe->netdev->priv_flags & IFF_802_1Q_VLAN &&
             fcoe->realdev->features & NETIF_F_HW_VLAN_TX) {
@@ -1917,7 +1917,6 @@ static int fcoe_dcb_app_notification(struct notifier_block *notifier,
         struct fcoe_ctlr *ctlr;
         struct fcoe_interface *fcoe;
         struct net_device *netdev;
-        struct fcoe_port *port;
         int prio;
 
         if (entry->app.selector != DCB_APP_IDTYPE_ETHTYPE)
@@ -1946,10 +1945,8 @@ static int fcoe_dcb_app_notification(struct notifier_block *notifier,
             entry->app.protocol == ETH_P_FCOE)
                 ctlr->priority = prio;
 
-        if (entry->app.protocol == ETH_P_FCOE) {
-                port = lport_priv(ctlr->lp);
-                port->priority = prio;
-        }
+        if (entry->app.protocol == ETH_P_FCOE)
+                fcoe->priority = prio;
 
         return NOTIFY_OK;
 }
@@ -2180,7 +2177,6 @@ static void fcoe_dcb_create(struct fcoe_interface *fcoe)
         u8 fup, up;
         struct net_device *netdev = fcoe->realdev;
         struct fcoe_ctlr *ctlr = fcoe_to_ctlr(fcoe);
-        struct fcoe_port *port = lport_priv(ctlr->lp);
         struct dcb_app app = {
                                 .priority = 0,
                                 .protocol = ETH_P_FCOE
@@ -2202,8 +2198,8 @@ static void fcoe_dcb_create(struct fcoe_interface *fcoe)
                         fup = dcb_getapp(netdev, &app);
                 }
 
-                port->priority = ffs(up) ? ffs(up) - 1 : 0;
-                ctlr->priority = ffs(fup) ? ffs(fup) - 1 : port->priority;
+                fcoe->priority = ffs(up) ? ffs(up) - 1 : 0;
+                ctlr->priority = ffs(fup) ? ffs(fup) - 1 : fcoe->priority;
         }
 #endif
 }
diff --git a/drivers/scsi/fcoe/fcoe.h b/drivers/scsi/fcoe/fcoe.h
index a624add4f8ec..b42dc32cb5eb 100644
--- a/drivers/scsi/fcoe/fcoe.h
+++ b/drivers/scsi/fcoe/fcoe.h
@@ -71,6 +71,7 @@ do {                                                            	\
  * @oem:              The offload exchange manager for all local port
  *                    instances associated with this port
  * @removed:          Indicates fcoe interface removed from net device
+ * @priority:         Priority for the FCoE packet (DCB)
  * This structure is 1:1 with a net device.
  */
 struct fcoe_interface {
@@ -81,6 +82,7 @@ struct fcoe_interface {
         struct packet_type fip_packet_type;
         struct fc_exch_mgr *oem;
         u8      removed;
+        u8      priority;
 };
 
 #define fcoe_to_ctlr(x)                                         \
diff --git a/drivers/scsi/libfc/fc_fcp.c b/drivers/scsi/libfc/fc_fcp.c
index 14243fa5f8e8..fcb9d0b20ee4 100644
--- a/drivers/scsi/libfc/fc_fcp.c
+++ b/drivers/scsi/libfc/fc_fcp.c
@@ -851,7 +851,8 @@ static void fc_fcp_resp(struct fc_fcp_pkt *fsp, struct fc_frame *fp)
                         fc_rp_info = (struct fcp_resp_rsp_info *)(rp_ex + 1);
                         if (flags & FCP_RSP_LEN_VAL) {
                                 respl = ntohl(rp_ex->fr_rsp_len);
-                                if (respl != sizeof(*fc_rp_info))
+                                if ((respl != FCP_RESP_RSP_INFO_LEN4) &&
+                                    (respl != FCP_RESP_RSP_INFO_LEN8))
                                         goto len_err;
                                 if (fsp->wait_for_comp) {
                                         /* Abuse cdb_status for rsp code */
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index a184c2443a64..69b59935b53f 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -27,6 +27,8 @@
 
 struct lpfc_sli2_slim;
 
+#define ELX_MODEL_NAME_SIZE     80
+
 #define LPFC_PCI_DEV_LP         0x1
 #define LPFC_PCI_DEV_OC         0x2
 
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index b032562aa0d9..ad16e54ac383 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -3935,6 +3935,12 @@ MODULE_PARM_DESC(lpfc_fcp_look_ahead, "Look ahead for completions");
 #       - Only meaningful if BG is turned on (lpfc_enable_bg=1).
 #       - Allows you to ultimately specify which profiles to use
 #       - Default will result in registering capabilities for all profiles.
+#       - SHOST_DIF_TYPE1_PROTECTION    1
+#               HBA supports T10 DIF Type 1: HBA to Target Type 1 Protection
+#       - SHOST_DIX_TYPE0_PROTECTION    8
+#               HBA supports DIX Type 0: Host to HBA protection only
+#       - SHOST_DIX_TYPE1_PROTECTION    16
+#               HBA supports DIX Type 1: Host to HBA  Type 1 protection
 #
 */
 unsigned int lpfc_prot_mask = SHOST_DIF_TYPE1_PROTECTION |
@@ -3947,7 +3953,7 @@ MODULE_PARM_DESC(lpfc_prot_mask, "host protection mask");
 /*
 # lpfc_prot_guard: i
 #       - Bit mask of protection guard types to register with the SCSI mid-layer
-#       - Guard types are currently either 1) IP checksum 2) T10-DIF CRC
+#       - Guard types are currently either 1) T10-DIF CRC 2) IP checksum
 #       - Allows you to ultimately specify which profiles to use
 #       - Default will result in registering capabilities for all guard types
 #
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index e470c489de07..4380a44000bc 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -467,3 +467,4 @@ int lpfc_sli4_read_config(struct lpfc_hba *);
 void lpfc_sli4_node_prep(struct lpfc_hba *);
 int lpfc_sli4_xri_sgl_update(struct lpfc_hba *);
 void lpfc_free_sgl_list(struct lpfc_hba *, struct list_head *);
+uint32_t lpfc_sli_port_speed_get(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index cfe533bc9790..f19e9b6f9f13 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -809,6 +809,8 @@ lpfc_cmpl_els_flogi_nport(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
         phba->fc_ratov = FF_DEF_RATOV;
         rc = memcmp(&vport->fc_portname, &sp->portName,
                     sizeof(vport->fc_portname));
+        memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
+
         if (rc >= 0) {
                 /* This side will initiate the PLOGI */
                 spin_lock_irq(shost->host_lock);
@@ -3160,7 +3162,8 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
                                 retry = 1;
                                 break;
                         }
-                        if (cmd == ELS_CMD_PLOGI) {
+                        if ((cmd == ELS_CMD_PLOGI) ||
+                            (cmd == ELS_CMD_PRLI)) {
                                 delay = 1000;
                                 maxretry = lpfc_max_els_tries + 1;
                                 retry = 1;
@@ -3305,7 +3308,7 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
                         ndlp->nlp_prev_state = ndlp->nlp_state;
                         if (cmd == ELS_CMD_PRLI)
                                 lpfc_nlp_set_state(vport, ndlp,
-                                        NLP_STE_REG_LOGIN_ISSUE);
+                                        NLP_STE_PRLI_ISSUE);
                         else
                                 lpfc_nlp_set_state(vport, ndlp,
                                         NLP_STE_NPR_NODE);
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index e9845d2ecf10..d7096ad94d3f 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -1506,9 +1506,10 @@ lpfc_match_fcf_conn_list(struct lpfc_hba *phba,
                 }
         }
 
-        /* If FCF not available return 0 */
+        /* FCF not valid/available or solicitation in progress */
         if (!bf_get(lpfc_fcf_record_fcf_avail, new_fcf_record) ||
-                !bf_get(lpfc_fcf_record_fcf_valid, new_fcf_record))
+            !bf_get(lpfc_fcf_record_fcf_valid, new_fcf_record) ||
+            bf_get(lpfc_fcf_record_fcf_sol, new_fcf_record))
                 return 0;
 
         if (!(phba->hba_flag & HBA_FIP_SUPPORT)) {
@@ -1842,6 +1843,7 @@ lpfc_sli4_log_fcf_record_info(struct lpfc_hba *phba,
                         "\tFCF_Index     : x%x\n"
                         "\tFCF_Avail     : x%x\n"
                         "\tFCF_Valid     : x%x\n"
+                        "\tFCF_SOL       : x%x\n"
                         "\tFIP_Priority  : x%x\n"
                         "\tMAC_Provider  : x%x\n"
                         "\tLowest VLANID : x%x\n"
@@ -1852,6 +1854,7 @@ lpfc_sli4_log_fcf_record_info(struct lpfc_hba *phba,
                         bf_get(lpfc_fcf_record_fcf_index, fcf_record),
                         bf_get(lpfc_fcf_record_fcf_avail, fcf_record),
                         bf_get(lpfc_fcf_record_fcf_valid, fcf_record),
+                        bf_get(lpfc_fcf_record_fcf_sol, fcf_record),
                         fcf_record->fip_priority,
                         bf_get(lpfc_fcf_record_mac_addr_prov, fcf_record),
                         vlan_id,
@@ -2185,12 +2188,14 @@ lpfc_mbx_cmpl_fcf_scan_read_fcf_rec(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
                                                new_fcf_record));
                 lpfc_printf_log(phba, KERN_WARNING, LOG_FIP,
                                 "2781 FCF (x%x) failed connection "
-                                "list check: (x%x/x%x)\n",
+                                "list check: (x%x/x%x/%x)\n",
                                 bf_get(lpfc_fcf_record_fcf_index,
                                        new_fcf_record),
                                 bf_get(lpfc_fcf_record_fcf_avail,
                                        new_fcf_record),
                                 bf_get(lpfc_fcf_record_fcf_valid,
+                                       new_fcf_record),
+                                bf_get(lpfc_fcf_record_fcf_sol,
                                        new_fcf_record));
                 if ((phba->fcf.fcf_flag & FCF_IN_USE) &&
                     lpfc_sli4_fcf_record_match(phba, &phba->fcf.current_rec,
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 834b699cac76..2cdeb5434fb7 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -1305,6 +1305,11 @@ struct lpfc_mbx_mq_create_ext {
 #define lpfc_mbx_mq_create_ext_async_evt_link_SHIFT     LPFC_TRAILER_CODE_LINK
 #define lpfc_mbx_mq_create_ext_async_evt_link_MASK      0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_link_WORD      async_evt_bmap
+#define LPFC_EVT_CODE_LINK_NO_LINK      0x0
+#define LPFC_EVT_CODE_LINK_10_MBIT      0x1
+#define LPFC_EVT_CODE_LINK_100_MBIT     0x2
+#define LPFC_EVT_CODE_LINK_1_GBIT       0x3
+#define LPFC_EVT_CODE_LINK_10_GBIT      0x4
 #define lpfc_mbx_mq_create_ext_async_evt_fip_SHIFT      LPFC_TRAILER_CODE_FCOE
 #define lpfc_mbx_mq_create_ext_async_evt_fip_MASK       0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_fip_WORD       async_evt_bmap
@@ -1314,6 +1319,13 @@ struct lpfc_mbx_mq_create_ext {
 #define lpfc_mbx_mq_create_ext_async_evt_fc_SHIFT       LPFC_TRAILER_CODE_FC
 #define lpfc_mbx_mq_create_ext_async_evt_fc_MASK        0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_fc_WORD        async_evt_bmap
+#define LPFC_EVT_CODE_FC_NO_LINK        0x0
+#define LPFC_EVT_CODE_FC_1_GBAUD        0x1
+#define LPFC_EVT_CODE_FC_2_GBAUD        0x2
+#define LPFC_EVT_CODE_FC_4_GBAUD        0x4
+#define LPFC_EVT_CODE_FC_8_GBAUD        0x8
+#define LPFC_EVT_CODE_FC_10_GBAUD       0xA
+#define LPFC_EVT_CODE_FC_16_GBAUD       0x10
 #define lpfc_mbx_mq_create_ext_async_evt_sli_SHIFT      LPFC_TRAILER_CODE_SLI
 #define lpfc_mbx_mq_create_ext_async_evt_sli_MASK       0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_sli_WORD       async_evt_bmap
@@ -1695,8 +1707,14 @@ struct fcf_record {
 #define lpfc_fcf_record_fc_map_2_MASK           0x000000FF
 #define lpfc_fcf_record_fc_map_2_WORD           word7
 #define lpfc_fcf_record_fcf_valid_SHIFT         24
-#define lpfc_fcf_record_fcf_valid_MASK          0x000000FF
+#define lpfc_fcf_record_fcf_valid_MASK          0x00000001
 #define lpfc_fcf_record_fcf_valid_WORD          word7
+#define lpfc_fcf_record_fcf_fc_SHIFT            25
+#define lpfc_fcf_record_fcf_fc_MASK             0x00000001
+#define lpfc_fcf_record_fcf_fc_WORD             word7
+#define lpfc_fcf_record_fcf_sol_SHIFT           31
+#define lpfc_fcf_record_fcf_sol_MASK            0x00000001
+#define lpfc_fcf_record_fcf_sol_WORD            word7
         uint32_t word8;
 #define lpfc_fcf_record_fcf_index_SHIFT         0
 #define lpfc_fcf_record_fcf_index_MASK          0x0000FFFF
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 8a55a586dd65..7dc4218d9c4c 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -1892,8 +1892,10 @@ lpfc_get_hba_model_desc(struct lpfc_hba *phba, uint8_t *mdp, uint8_t *descp)
                 max_speed = 4;
         else if (phba->lmt & LMT_2Gb)
                 max_speed = 2;
-        else
+        else if (phba->lmt & LMT_1Gb)
                 max_speed = 1;
+        else
+                max_speed = 0;
 
         vp = &phba->vpd;
 
@@ -2078,9 +2080,13 @@ lpfc_get_hba_model_desc(struct lpfc_hba *phba, uint8_t *mdp, uint8_t *descp)
         if (descp && descp[0] == '\0') {
                 if (oneConnect)
                         snprintf(descp, 255,
-                                "Emulex OneConnect %s, %s Initiator, Port %s",
+                                "Emulex OneConnect %s, %s Initiator %s",
                                 m.name, m.function,
                                 phba->Port);
+                else if (max_speed == 0)
+                        snprintf(descp, 255,
+                                "Emulex %s %s %s ",
+                                m.name, m.bus, m.function);
                 else
                         snprintf(descp, 255,
                                 "Emulex %s %d%s %s %s",
@@ -3502,6 +3508,119 @@ lpfc_sli4_parse_latt_link_speed(struct lpfc_hba *phba,
 }
 
 /**
+ * lpfc_sli_port_speed_get - Get sli3 link speed code to link speed
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine is to get an SLI3 FC port's link speed in Mbps.
+ *
+ * Return: link speed in terms of Mbps.
+ **/
+uint32_t
+lpfc_sli_port_speed_get(struct lpfc_hba *phba)
+{
+        uint32_t link_speed;
+
+        if (!lpfc_is_link_up(phba))
+                return 0;
+
+        switch (phba->fc_linkspeed) {
+        case LPFC_LINK_SPEED_1GHZ:
+                link_speed = 1000;
+                break;
+        case LPFC_LINK_SPEED_2GHZ:
+                link_speed = 2000;
+                break;
+        case LPFC_LINK_SPEED_4GHZ:
+                link_speed = 4000;
+                break;
+        case LPFC_LINK_SPEED_8GHZ:
+                link_speed = 8000;
+                break;
+        case LPFC_LINK_SPEED_10GHZ:
+                link_speed = 10000;
+                break;
+        case LPFC_LINK_SPEED_16GHZ:
+                link_speed = 16000;
+                break;
+        default:
+                link_speed = 0;
+        }
+        return link_speed;
+}
+
+/**
+ * lpfc_sli4_port_speed_parse - Parse async evt link speed code to link speed
+ * @phba: pointer to lpfc hba data structure.
+ * @evt_code: asynchronous event code.
+ * @speed_code: asynchronous event link speed code.
+ *
+ * This routine is to parse the giving SLI4 async event link speed code into
+ * value of Mbps for the link speed.
+ *
+ * Return: link speed in terms of Mbps.
+ **/
+static uint32_t
+lpfc_sli4_port_speed_parse(struct lpfc_hba *phba, uint32_t evt_code,
+                           uint8_t speed_code)
+{
+        uint32_t port_speed;
+
+        switch (evt_code) {
+        case LPFC_TRAILER_CODE_LINK:
+                switch (speed_code) {
+                case LPFC_EVT_CODE_LINK_NO_LINK:
+                        port_speed = 0;
+                        break;
+                case LPFC_EVT_CODE_LINK_10_MBIT:
+                        port_speed = 10;
+                        break;
+                case LPFC_EVT_CODE_LINK_100_MBIT:
+                        port_speed = 100;
+                        break;
+                case LPFC_EVT_CODE_LINK_1_GBIT:
+                        port_speed = 1000;
+                        break;
+                case LPFC_EVT_CODE_LINK_10_GBIT:
+                        port_speed = 10000;
+                        break;
+                default:
+                        port_speed = 0;
+                }
+                break;
+        case LPFC_TRAILER_CODE_FC:
+                switch (speed_code) {
+                case LPFC_EVT_CODE_FC_NO_LINK:
+                        port_speed = 0;
+                        break;
+                case LPFC_EVT_CODE_FC_1_GBAUD:
+                        port_speed = 1000;
+                        break;
+                case LPFC_EVT_CODE_FC_2_GBAUD:
+                        port_speed = 2000;
+                        break;
+                case LPFC_EVT_CODE_FC_4_GBAUD:
+                        port_speed = 4000;
+                        break;
+                case LPFC_EVT_CODE_FC_8_GBAUD:
+                        port_speed = 8000;
+                        break;
+                case LPFC_EVT_CODE_FC_10_GBAUD:
+                        port_speed = 10000;
+                        break;
+                case LPFC_EVT_CODE_FC_16_GBAUD:
+                        port_speed = 16000;
+                        break;
+                default:
+                        port_speed = 0;
+                }
+                break;
+        default:
+                port_speed = 0;
+        }
+        return port_speed;
+}
+
+/**
  * lpfc_sli4_async_link_evt - Process the asynchronous FCoE link event
  * @phba: pointer to lpfc hba data structure.
  * @acqe_link: pointer to the async link completion queue entry.
@@ -3558,7 +3677,8 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
 
         /* Keep the link status for extra SLI4 state machine reference */
         phba->sli4_hba.link_state.speed =
-                                bf_get(lpfc_acqe_link_speed, acqe_link);
+                        lpfc_sli4_port_speed_parse(phba, LPFC_TRAILER_CODE_LINK,
+                                bf_get(lpfc_acqe_link_speed, acqe_link));
         phba->sli4_hba.link_state.duplex =
                                 bf_get(lpfc_acqe_link_duplex, acqe_link);
         phba->sli4_hba.link_state.status =
@@ -3570,7 +3690,8 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
         phba->sli4_hba.link_state.fault =
                                 bf_get(lpfc_acqe_link_fault, acqe_link);
         phba->sli4_hba.link_state.logical_speed =
-                        bf_get(lpfc_acqe_logical_link_speed, acqe_link);
+                        bf_get(lpfc_acqe_logical_link_speed, acqe_link) * 10;
+
         lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
                         "2900 Async FC/FCoE Link event - Speed:%dGBit "
                         "duplex:x%x LA Type:x%x Port Type:%d Port Number:%d "
@@ -3580,7 +3701,7 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
                         phba->sli4_hba.link_state.status,
                         phba->sli4_hba.link_state.type,
                         phba->sli4_hba.link_state.number,
-                        phba->sli4_hba.link_state.logical_speed * 10,
+                        phba->sli4_hba.link_state.logical_speed,
                         phba->sli4_hba.link_state.fault);
         /*
          * For FC Mode: issue the READ_TOPOLOGY mailbox command to fetch
@@ -3652,7 +3773,8 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
         }
         /* Keep the link status for extra SLI4 state machine reference */
         phba->sli4_hba.link_state.speed =
-                                bf_get(lpfc_acqe_fc_la_speed, acqe_fc);
+                        lpfc_sli4_port_speed_parse(phba, LPFC_TRAILER_CODE_FC,
+                                bf_get(lpfc_acqe_fc_la_speed, acqe_fc));
         phba->sli4_hba.link_state.duplex = LPFC_ASYNC_LINK_DUPLEX_FULL;
         phba->sli4_hba.link_state.topology =
                                 bf_get(lpfc_acqe_fc_la_topology, acqe_fc);
@@ -3665,7 +3787,7 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
         phba->sli4_hba.link_state.fault =
                                 bf_get(lpfc_acqe_link_fault, acqe_fc);
         phba->sli4_hba.link_state.logical_speed =
-                                bf_get(lpfc_acqe_fc_la_llink_spd, acqe_fc);
+                                bf_get(lpfc_acqe_fc_la_llink_spd, acqe_fc) * 10;
         lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
                         "2896 Async FC event - Speed:%dGBaud Topology:x%x "
                         "LA Type:x%x Port Type:%d Port Number:%d Logical speed:"
@@ -3675,7 +3797,7 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
                         phba->sli4_hba.link_state.status,
                         phba->sli4_hba.link_state.type,
                         phba->sli4_hba.link_state.number,
-                        phba->sli4_hba.link_state.logical_speed * 10,
+                        phba->sli4_hba.link_state.logical_speed,
                         phba->sli4_hba.link_state.fault);
         pmb = (LPFC_MBOXQ_t *)mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
         if (!pmb) {
@@ -3783,14 +3905,18 @@ lpfc_sli4_async_sli_evt(struct lpfc_hba *phba, struct lpfc_acqe_sli *acqe_sli)
         case LPFC_SLI_EVENT_STATUS_VALID:
                 return; /* no message if the sfp is okay */
         case LPFC_SLI_EVENT_STATUS_NOT_PRESENT:
-                sprintf(message, "Not installed");
+                sprintf(message, "Optics faulted/incorrectly installed/not " \
+                                "installed - Reseat optics, if issue not "
+                                "resolved, replace.");
                 break;
         case LPFC_SLI_EVENT_STATUS_WRONG_TYPE:
                 sprintf(message,
-                        "Optics of two types installed");
+                        "Optics of two types installed - Remove one optic or " \
+                        "install matching pair of optics.");
                 break;
         case LPFC_SLI_EVENT_STATUS_UNSUPPORTED:
-                sprintf(message, "Incompatible optics");
+                sprintf(message, "Incompatible optics - Replace with " \
+                                "compatible optics for card to function.");
                 break;
         default:
                 /* firmware is reporting a status we don't know about */
@@ -4161,11 +4287,11 @@ lpfc_sli4_async_grp5_evt(struct lpfc_hba *phba,
         phba->fcoe_eventtag = acqe_grp5->event_tag;
         prev_ll_spd = phba->sli4_hba.link_state.logical_speed;
         phba->sli4_hba.link_state.logical_speed =
-                (bf_get(lpfc_acqe_grp5_llink_spd, acqe_grp5));
+                (bf_get(lpfc_acqe_grp5_llink_spd, acqe_grp5)) * 10;
         lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
                         "2789 GRP5 Async Event: Updating logical link speed "
-                        "from %dMbps to %dMbps\n", (prev_ll_spd * 10),
-                        (phba->sli4_hba.link_state.logical_speed*10));
+                        "from %dMbps to %dMbps\n", prev_ll_spd,
+                        phba->sli4_hba.link_state.logical_speed);
 }
 
 /**
@@ -4947,7 +5073,7 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
         }
 
         phba->sli4_hba.msix_entries = kzalloc((sizeof(struct msix_entry) *
-                                      phba->sli4_hba.cfg_eqn), GFP_KERNEL);
+                                      phba->cfg_fcp_io_channel), GFP_KERNEL);
         if (!phba->sli4_hba.msix_entries) {
                 lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
                                 "2573 Failed allocate memory for msi-x "
@@ -6559,7 +6685,8 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
                 i++;
         }
         if (i < cfg_fcp_io_channel) {
-                lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
+                lpfc_printf_log(phba,
+                                KERN_ERR, LOG_INIT,
                                 "3188 Reducing IO channels to match number of "
                                 "CPUs: from %d to %d\n", cfg_fcp_io_channel, i);
                 cfg_fcp_io_channel = i;
@@ -6567,8 +6694,8 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
 
         if (cfg_fcp_io_channel >
             phba->sli4_hba.max_cfg_param.max_eq) {
-                cfg_fcp_io_channel = phba->sli4_hba.max_cfg_param.max_eq;
-                if (cfg_fcp_io_channel < LPFC_FCP_IO_CHAN_MIN) {
+                if (phba->sli4_hba.max_cfg_param.max_eq <
+                    LPFC_FCP_IO_CHAN_MIN) {
                         lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
                                         "2574 Not enough EQs (%d) from the "
                                         "pci function for supporting FCP "
@@ -6577,13 +6704,12 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
                                         phba->cfg_fcp_io_channel);
                         goto out_error;
                 }
-                lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
-                                "2575 Not enough EQs (%d) from the pci "
-                                "function for supporting the requested "
-                                "FCP EQs (%d), the actual FCP EQs can "
-                                "be supported: %d\n",
-                                phba->sli4_hba.max_cfg_param.max_eq,
-                                phba->cfg_fcp_io_channel, cfg_fcp_io_channel);
+                lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+                                "2575 Reducing IO channels to match number of "
+                                "available EQs: from %d to %d\n",
+                                cfg_fcp_io_channel,
+                                phba->sli4_hba.max_cfg_param.max_eq);
+                cfg_fcp_io_channel = phba->sli4_hba.max_cfg_param.max_eq;
         }
 
         /* Eventually cfg_fcp_eq_count / cfg_fcp_wq_count will be depricated */
@@ -6592,7 +6718,6 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
         phba->cfg_fcp_eq_count = cfg_fcp_io_channel;
         phba->cfg_fcp_wq_count = cfg_fcp_io_channel;
         phba->cfg_fcp_io_channel = cfg_fcp_io_channel;
-        phba->sli4_hba.cfg_eqn = cfg_fcp_io_channel;
 
         /* Get EQ depth from module parameter, fake the default for now */
         phba->sli4_hba.eq_esize = LPFC_EQE_SIZE_4B;
@@ -8095,11 +8220,11 @@ lpfc_sli4_enable_msix(struct lpfc_hba *phba)
         int vectors, rc, index;
 
         /* Set up MSI-X multi-message vectors */
-        for (index = 0; index < phba->sli4_hba.cfg_eqn; index++)
+        for (index = 0; index < phba->cfg_fcp_io_channel; index++)
                 phba->sli4_hba.msix_entries[index].entry = index;
 
         /* Configure MSI-X capability structure */
-        vectors = phba->sli4_hba.cfg_eqn;
+        vectors = phba->cfg_fcp_io_channel;
 enable_msix_vectors:
         rc = pci_enable_msix(phba->pcidev, phba->sli4_hba.msix_entries,
                              vectors);
@@ -8142,8 +8267,14 @@ enable_msix_vectors:
                         goto cfg_fail_out;
                 }
         }
-        phba->sli4_hba.msix_vec_nr = vectors;
 
+        if (vectors != phba->cfg_fcp_io_channel) {
+                lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+                                "3238 Reducing IO channels to match number of "
+                                "MSI-X vectors, requested %d got %d\n",
+                                phba->cfg_fcp_io_channel, vectors);
+                phba->cfg_fcp_io_channel = vectors;
+        }
         return rc;
 
 cfg_fail_out:
@@ -8171,7 +8302,7 @@ lpfc_sli4_disable_msix(struct lpfc_hba *phba)
         int index;
 
         /* Free up MSI-X multi-message vectors */
-        for (index = 0; index < phba->sli4_hba.msix_vec_nr; index++)
+        for (index = 0; index < phba->cfg_fcp_io_channel; index++)
                 free_irq(phba->sli4_hba.msix_entries[index].vector,
                          &phba->sli4_hba.fcp_eq_hdl[index]);
 
@@ -9304,23 +9435,28 @@ lpfc_sli4_get_els_iocb_cnt(struct lpfc_hba *phba)
 
 /**
  * lpfc_write_firmware - attempt to write a firmware image to the port
- * @phba: pointer to lpfc hba data structure.
  * @fw: pointer to firmware image returned from request_firmware.
+ * @phba: pointer to lpfc hba data structure.
  *
- * returns the number of bytes written if write is successful.
- * returns a negative error value if there were errors.
- * returns 0 if firmware matches currently active firmware on port.
  **/
-int
-lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
+static void
+lpfc_write_firmware(const struct firmware *fw, void *context)
 {
+        struct lpfc_hba *phba = (struct lpfc_hba *)context;
         char fwrev[FW_REV_STR_SIZE];
-        struct lpfc_grp_hdr *image = (struct lpfc_grp_hdr *)fw->data;
+        struct lpfc_grp_hdr *image;
         struct list_head dma_buffer_list;
         int i, rc = 0;
         struct lpfc_dmabuf *dmabuf, *next;
         uint32_t offset = 0, temp_offset = 0;
 
+        /* It can be null, sanity check */
+        if (!fw) {
+                rc = -ENXIO;
+                goto out;
+        }
+        image = (struct lpfc_grp_hdr *)fw->data;
+
         INIT_LIST_HEAD(&dma_buffer_list);
         if ((be32_to_cpu(image->magic_number) != LPFC_GROUP_OJECT_MAGIC_NUM) ||
             (bf_get_be32(lpfc_grp_hdr_file_type, image) !=
@@ -9333,12 +9469,13 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
                                 be32_to_cpu(image->magic_number),
                                 bf_get_be32(lpfc_grp_hdr_file_type, image),
                                 bf_get_be32(lpfc_grp_hdr_id, image));
-                return -EINVAL;
+                rc = -EINVAL;
+                goto release_out;
         }
         lpfc_decode_firmware_rev(phba, fwrev, 1);
         if (strncmp(fwrev, image->revision, strnlen(image->revision, 16))) {
                 lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-                                "3023 Updating Firmware. Current Version:%s "
+                                "3023 Updating Firmware, Current Version:%s "
                                 "New Version:%s\n",
                                 fwrev, image->revision);
                 for (i = 0; i < LPFC_MBX_WR_CONFIG_MAX_BDE; i++) {
@@ -9346,7 +9483,7 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
                                          GFP_KERNEL);
                         if (!dmabuf) {
                                 rc = -ENOMEM;
-                                goto out;
+                                goto release_out;
                         }
                         dmabuf->virt = dma_alloc_coherent(&phba->pcidev->dev,
                                                           SLI4_PAGE_SIZE,
@@ -9355,7 +9492,7 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
                         if (!dmabuf->virt) {
                                 kfree(dmabuf);
                                 rc = -ENOMEM;
-                                goto out;
+                                goto release_out;
                         }
                         list_add_tail(&dmabuf->list, &dma_buffer_list);
                 }
@@ -9375,23 +9512,24 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
                         }
                         rc = lpfc_wr_object(phba, &dma_buffer_list,
                                     (fw->size - offset), &offset);
-                        if (rc) {
-                                lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-                                                "3024 Firmware update failed. "
-                                                "%d\n", rc);
-                                goto out;
-                        }
+                        if (rc)
+                                goto release_out;
                 }
                 rc = offset;
         }
-out:
+
+release_out:
         list_for_each_entry_safe(dmabuf, next, &dma_buffer_list, list) {
                 list_del(&dmabuf->list);
                 dma_free_coherent(&phba->pcidev->dev, SLI4_PAGE_SIZE,
                                   dmabuf->virt, dmabuf->phys);
                 kfree(dmabuf);
         }
-        return rc;
+        release_firmware(fw);
+out:
+        lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+                        "3024 Firmware update done: %d.", rc);
+        return;
 }
 
 /**
@@ -9418,12 +9556,11 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
         struct lpfc_hba   *phba;
         struct lpfc_vport *vport = NULL;
         struct Scsi_Host  *shost = NULL;
-        int error;
+        int error, ret;
         uint32_t cfg_mode, intr_mode;
         int mcnt;
         int adjusted_fcp_io_channel;
-        const struct firmware *fw;
-        uint8_t file_name[16];
+        uint8_t file_name[ELX_MODEL_NAME_SIZE];
 
         /* Allocate memory for HBA structure */
         phba = lpfc_hba_alloc(pdev);
@@ -9525,9 +9662,6 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
                 /* Default to single EQ for non-MSI-X */
                 if (phba->intr_type != MSIX)
                         adjusted_fcp_io_channel = 1;
-                else if (phba->sli4_hba.msix_vec_nr <
-                                        phba->cfg_fcp_io_channel)
-                        adjusted_fcp_io_channel = phba->sli4_hba.msix_vec_nr;
                 else
                         adjusted_fcp_io_channel = phba->cfg_fcp_io_channel;
                 phba->cfg_fcp_io_channel = adjusted_fcp_io_channel;
@@ -9572,12 +9706,12 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
         /* check for firmware upgrade or downgrade (if_type 2 only) */
         if (bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) ==
             LPFC_SLI_INTF_IF_TYPE_2) {
-                snprintf(file_name, 16, "%s.grp", phba->ModelName);
-                error = request_firmware(&fw, file_name, &phba->pcidev->dev);
-                if (!error) {
-                        lpfc_write_firmware(phba, fw);
-                        release_firmware(fw);
-                }
+                snprintf(file_name, ELX_MODEL_NAME_SIZE, "%s.grp",
+                         phba->ModelName);
+                ret = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+                                        file_name, &phba->pcidev->dev,
+                                        GFP_KERNEL, (void *)phba,
+                                        lpfc_write_firmware);
         }
 
         /* Check if there are static vports to be created. */
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 64013f3097ad..7f45ac9964a9 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -3829,9 +3829,9 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
         cmd->scsi_done(cmd);
 
         if (phba->cfg_poll & ENABLE_FCP_RING_POLLING) {
-                spin_lock_irq(&phba->hbalock);
+                spin_lock_irqsave(&phba->hbalock, flags);
                 lpfc_cmd->pCmd = NULL;
-                spin_unlock_irq(&phba->hbalock);
+                spin_unlock_irqrestore(&phba->hbalock, flags);
 
                 /*
                  * If there is a thread waiting for command completion
@@ -3871,9 +3871,9 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
                 }
         }
 
-        spin_lock_irq(&phba->hbalock);
+        spin_lock_irqsave(&phba->hbalock, flags);
         lpfc_cmd->pCmd = NULL;
-        spin_unlock_irq(&phba->hbalock);
+        spin_unlock_irqrestore(&phba->hbalock, flags);
 
         /*
          * If there is a thread waiting for command completion
@@ -4163,7 +4163,7 @@ lpfc_info(struct Scsi_Host *host)
 {
         struct lpfc_vport *vport = (struct lpfc_vport *) host->hostdata;
         struct lpfc_hba   *phba = vport->phba;
-        int len;
+        int len, link_speed = 0;
         static char  lpfcinfobuf[384];
 
         memset(lpfcinfobuf,0,384);
@@ -4184,12 +4184,18 @@ lpfc_info(struct Scsi_Host *host)
                                  phba->Port);
                 }
                 len = strlen(lpfcinfobuf);
-                if (phba->sli4_hba.link_state.logical_speed) {
-                        snprintf(lpfcinfobuf + len,
-                                 384-len,
-                                 " Logical Link Speed: %d Mbps",
-                                 phba->sli4_hba.link_state.logical_speed * 10);
+                if (phba->sli_rev <= LPFC_SLI_REV3) {
+                        link_speed = lpfc_sli_port_speed_get(phba);
+                } else {
+                        if (phba->sli4_hba.link_state.logical_speed)
+                                link_speed =
+                                      phba->sli4_hba.link_state.logical_speed;
+                        else
+                                link_speed = phba->sli4_hba.link_state.speed;
                 }
+                if (link_speed != 0)
+                        snprintf(lpfcinfobuf + len, 384-len,
+                                 " Logical Link Speed: %d Mbps", link_speed);
         }
         return lpfcinfobuf;
 }
@@ -4398,16 +4404,17 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
         struct lpfc_scsi_buf *lpfc_cmd;
         IOCB_t *cmd, *icmd;
         int ret = SUCCESS, status = 0;
+        unsigned long flags;
         DECLARE_WAIT_QUEUE_HEAD_ONSTACK(waitq);
 
         status = fc_block_scsi_eh(cmnd);
         if (status != 0 && status != SUCCESS)
                 return status;
 
-        spin_lock_irq(&phba->hbalock);
+        spin_lock_irqsave(&phba->hbalock, flags);
         /* driver queued commands are in process of being flushed */
         if (phba->hba_flag & HBA_FCP_IOQ_FLUSH) {
-                spin_unlock_irq(&phba->hbalock);
+                spin_unlock_irqrestore(&phba->hbalock, flags);
                 lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
                         "3168 SCSI Layer abort requested I/O has been "
                         "flushed by LLD.\n");
@@ -4416,7 +4423,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 
         lpfc_cmd = (struct lpfc_scsi_buf *)cmnd->host_scribble;
         if (!lpfc_cmd || !lpfc_cmd->pCmd) {
-                spin_unlock_irq(&phba->hbalock);
+                spin_unlock_irqrestore(&phba->hbalock, flags);
                 lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
                          "2873 SCSI Layer I/O Abort Request IO CMPL Status "
                          "x%x ID %d LUN %d\n",
@@ -4427,7 +4434,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
         iocb = &lpfc_cmd->cur_iocbq;
         /* the command is in process of being cancelled */
         if (!(iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ)) {
-                spin_unlock_irq(&phba->hbalock);
+                spin_unlock_irqrestore(&phba->hbalock, flags);
                 lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
                         "3169 SCSI Layer abort requested I/O has been "
                         "cancelled by LLD.\n");
@@ -4484,7 +4491,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
         abtsiocb->iocb_cmpl = lpfc_sli_abort_fcp_cmpl;
         abtsiocb->vport = vport;
         /* no longer need the lock after this point */
-        spin_unlock_irq(&phba->hbalock);
+        spin_unlock_irqrestore(&phba->hbalock, flags);
 
         if (lpfc_sli_issue_iocb(phba, LPFC_FCP_RING, abtsiocb, 0) ==
             IOCB_ERROR) {
@@ -4516,7 +4523,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
         goto out;
 
 out_unlock:
-        spin_unlock_irq(&phba->hbalock);
+        spin_unlock_irqrestore(&phba->hbalock, flags);
 out:
         lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
                          "0749 SCSI Layer I/O Abort Request Status x%x ID %d "
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 219bf534ef99..d7f3313ef886 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -3964,9 +3964,9 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
         pci_write_config_word(phba->pcidev, PCI_COMMAND, (cfg_value &
                               ~(PCI_COMMAND_PARITY | PCI_COMMAND_SERR)));
 
-        /* Perform FCoE PCI function reset */
-        lpfc_sli4_queue_destroy(phba);
+        /* Perform FCoE PCI function reset before freeing queue memory */
         rc = lpfc_pci_function_reset(phba);
+        lpfc_sli4_queue_destroy(phba);
 
         /* Restore PCI cmd register */
         pci_write_config_word(phba->pcidev, PCI_COMMAND, cfg_value);
@@ -7072,6 +7072,40 @@ lpfc_sli4_async_mbox_unblock(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_sli4_wait_bmbx_ready - Wait for bootstrap mailbox register ready
+ * @phba: Pointer to HBA context object.
+ * @mboxq: Pointer to mailbox object.
+ *
+ * The function waits for the bootstrap mailbox register ready bit from
+ * port for twice the regular mailbox command timeout value.
+ *
+ *      0 - no timeout on waiting for bootstrap mailbox register ready.
+ *      MBXERR_ERROR - wait for bootstrap mailbox register timed out.
+ **/
+static int
+lpfc_sli4_wait_bmbx_ready(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
+{
+        uint32_t db_ready;
+        unsigned long timeout;
+        struct lpfc_register bmbx_reg;
+
+        timeout = msecs_to_jiffies(lpfc_mbox_tmo_val(phba, mboxq)
+                                   * 1000) + jiffies;
+
+        do {
+                bmbx_reg.word0 = readl(phba->sli4_hba.BMBXregaddr);
+                db_ready = bf_get(lpfc_bmbx_rdy, &bmbx_reg);
+                if (!db_ready)
+                        msleep(2);
+
+                if (time_after(jiffies, timeout))
+                        return MBXERR_ERROR;
+        } while (!db_ready);
+
+        return 0;
+}
+
+/**
  * lpfc_sli4_post_sync_mbox - Post an SLI4 mailbox to the bootstrap mailbox
  * @phba: Pointer to HBA context object.
  * @mboxq: Pointer to mailbox object.
@@ -7092,15 +7126,12 @@ lpfc_sli4_post_sync_mbox(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 {
         int rc = MBX_SUCCESS;
         unsigned long iflag;
-        uint32_t db_ready;
         uint32_t mcqe_status;
         uint32_t mbx_cmnd;
-        unsigned long timeout;
         struct lpfc_sli *psli = &phba->sli;
         struct lpfc_mqe *mb = &mboxq->u.mqe;
         struct lpfc_bmbx_create *mbox_rgn;
         struct dma_address *dma_address;
-        struct lpfc_register bmbx_reg;
 
         /*
          * Only one mailbox can be active to the bootstrap mailbox region
@@ -7124,6 +7155,11 @@ lpfc_sli4_post_sync_mbox(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
         phba->sli.mbox_active = mboxq;
         spin_unlock_irqrestore(&phba->hbalock, iflag);
 
+        /* wait for bootstrap mbox register for readyness */
+        rc = lpfc_sli4_wait_bmbx_ready(phba, mboxq);
+        if (rc)
+                goto exit;
+
         /*
          * Initialize the bootstrap memory region to avoid stale data areas
          * in the mailbox post.  Then copy the caller's mailbox contents to
@@ -7138,35 +7174,18 @@ lpfc_sli4_post_sync_mbox(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
         dma_address = &phba->sli4_hba.bmbx.dma_address;
         writel(dma_address->addr_hi, phba->sli4_hba.BMBXregaddr);
 
-        timeout = msecs_to_jiffies(lpfc_mbox_tmo_val(phba, mboxq)
-                                   * 1000) + jiffies;
-        do {
-                bmbx_reg.word0 = readl(phba->sli4_hba.BMBXregaddr);
-                db_ready = bf_get(lpfc_bmbx_rdy, &bmbx_reg);
-                if (!db_ready)
-                        msleep(2);
-
-                if (time_after(jiffies, timeout)) {
-                        rc = MBXERR_ERROR;
-                        goto exit;
-                }
-        } while (!db_ready);
+        /* wait for bootstrap mbox register for hi-address write done */
+        rc = lpfc_sli4_wait_bmbx_ready(phba, mboxq);
+        if (rc)
+                goto exit;
 
         /* Post the low mailbox dma address to the port. */
         writel(dma_address->addr_lo, phba->sli4_hba.BMBXregaddr);
-        timeout = msecs_to_jiffies(lpfc_mbox_tmo_val(phba, mboxq)
-                                   * 1000) + jiffies;
-        do {
-                bmbx_reg.word0 = readl(phba->sli4_hba.BMBXregaddr);
-                db_ready = bf_get(lpfc_bmbx_rdy, &bmbx_reg);
-                if (!db_ready)
-                        msleep(2);
 
-                if (time_after(jiffies, timeout)) {
-                        rc = MBXERR_ERROR;
-                        goto exit;
-                }
-        } while (!db_ready);
+        /* wait for bootstrap mbox register for low address write done */
+        rc = lpfc_sli4_wait_bmbx_ready(phba, mboxq);
+        if (rc)
+                goto exit;
 
         /*
          * Read the CQ to ensure the mailbox has completed.
@@ -8090,6 +8109,8 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
                 bf_set(wqe_lenloc, &wqe->fcp_icmd.wqe_com,
                        LPFC_WQE_LENLOC_NONE);
                 bf_set(wqe_ebde_cnt, &wqe->fcp_icmd.wqe_com, 0);
+                bf_set(wqe_erp, &wqe->fcp_icmd.wqe_com,
+                       iocbq->iocb.ulpFCP2Rcvy);
                 break;
         case CMD_GEN_REQUEST64_CR:
                 /* For this command calculate the xmit length of the
@@ -12099,6 +12120,7 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
         struct lpfc_queue *eq;
         int cnt, rc, length, status = 0;
         uint32_t shdr_status, shdr_add_status;
+        uint32_t result;
         int fcp_eqidx;
         union lpfc_sli4_cfg_shdr *shdr;
         uint16_t dmult;
@@ -12117,8 +12139,11 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
         eq_delay = &mbox->u.mqe.un.eq_delay;
 
         /* Calculate delay multiper from maximum interrupt per second */
-        dmult = phba->cfg_fcp_imax / phba->cfg_fcp_io_channel;
-        dmult = LPFC_DMULT_CONST/dmult - 1;
+        result = phba->cfg_fcp_imax / phba->cfg_fcp_io_channel;
+        if (result > LPFC_DMULT_CONST)
+                dmult = 0;
+        else
+                dmult = LPFC_DMULT_CONST/result - 1;
 
         cnt = 0;
         for (fcp_eqidx = startq; fcp_eqidx < phba->cfg_fcp_io_channel;
@@ -12174,7 +12199,7 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
  * fails this function will return -ENXIO.
  **/
 uint32_t
-lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint16_t imax)
+lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint32_t imax)
 {
         struct lpfc_mbx_eq_create *eq_create;
         LPFC_MBOXQ_t *mbox;
@@ -12206,7 +12231,10 @@ lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint16_t imax)
                LPFC_EQE_SIZE);
         bf_set(lpfc_eq_context_valid, &eq_create->u.request.context, 1);
         /* Calculate delay multiper from maximum interrupt per second */
-        dmult = LPFC_DMULT_CONST/imax - 1;
+        if (imax > LPFC_DMULT_CONST)
+                dmult = 0;
+        else
+                dmult = LPFC_DMULT_CONST/imax - 1;
         bf_set(lpfc_eq_context_delay_multi, &eq_create->u.request.context,
                dmult);
         switch (eq->entry_count) {
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index bd4bc4342ae2..f44a06a4c6e7 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -37,7 +37,7 @@
 /* Multi-queue arrangement for FCP EQ/CQ/WQ tuples */
 #define LPFC_FCP_IO_CHAN_DEF       4
 #define LPFC_FCP_IO_CHAN_MIN       1
-#define LPFC_FCP_IO_CHAN_MAX       8
+#define LPFC_FCP_IO_CHAN_MAX       16
 
 /*
  * Provide the default FCF Record attributes used by the driver
@@ -168,7 +168,7 @@ struct lpfc_queue {
 };
 
 struct lpfc_sli4_link {
-        uint8_t speed;
+        uint16_t speed;
         uint8_t duplex;
         uint8_t status;
         uint8_t type;
@@ -490,8 +490,6 @@ struct lpfc_sli4_hba {
         struct lpfc_pc_sli4_params pc_sli4_params;
         struct msix_entry *msix_entries;
         uint8_t handler_name[LPFC_FCP_IO_CHAN_MAX][LPFC_SLI4_HANDLER_NAME_SZ];
-        uint32_t cfg_eqn;
-        uint32_t msix_vec_nr;
         struct lpfc_fcp_eq_hdl *fcp_eq_hdl; /* FCP per-WQ handle */
 
         /* Pointers to the constructed SLI4 queues */
@@ -626,7 +624,7 @@ void lpfc_sli4_hba_reset(struct lpfc_hba *);
 struct lpfc_queue *lpfc_sli4_queue_alloc(struct lpfc_hba *, uint32_t,
                         uint32_t);
 void lpfc_sli4_queue_free(struct lpfc_queue *);
-uint32_t lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint16_t);
+uint32_t lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint32_t);
 uint32_t lpfc_modify_fcp_eq_delay(struct lpfc_hba *, uint16_t);
 uint32_t lpfc_cq_create(struct lpfc_hba *, struct lpfc_queue *,
                         struct lpfc_queue *, uint32_t, uint32_t);
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 04265a1c4e52..0c2149189dda 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "8.3.34"
+#define LPFC_DRIVER_VERSION "8.3.35"
 #define LPFC_DRIVER_NAME                "lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index fcb005fa4bd1..16b7a72a70c4 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2003-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -33,9 +33,9 @@
 /*
  * MegaRAID SAS Driver meta data
  */
-#define MEGASAS_VERSION                         "00.00.06.18-rc1"
-#define MEGASAS_RELDATE                         "Jun. 17, 2012"
-#define MEGASAS_EXT_VERSION                     "Tue. Jun. 17 17:00:00 PDT 2012"
+#define MEGASAS_VERSION                         "06.504.01.00-rc1"
+#define MEGASAS_RELDATE                         "Oct. 1, 2012"
+#define MEGASAS_EXT_VERSION                     "Mon. Oct. 1 17:00:00 PDT 2012"
 
 /*
  * Device IDs
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 0393ec478cdf..d2c5366aff7f 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2003-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -18,7 +18,7 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  *
  *  FILE: megaraid_sas_base.c
- *  Version : v00.00.06.18-rc1
+ *  Version : v06.504.01.00-rc1
  *
  *  Authors: LSI Corporation
  *           Sreenivas Bagalkote
@@ -71,6 +71,10 @@ static int msix_disable;
 module_param(msix_disable, int, S_IRUGO);
 MODULE_PARM_DESC(msix_disable, "Disable MSI-X interrupt handling. Default: 0");
 
+static unsigned int msix_vectors;
+module_param(msix_vectors, int, S_IRUGO);
+MODULE_PARM_DESC(msix_vectors, "MSI-X max vector count. Default: Set by FW");
+
 static int throttlequeuedepth = MEGASAS_THROTTLE_QUEUE_DEPTH;
 module_param(throttlequeuedepth, int, S_IRUGO);
 MODULE_PARM_DESC(throttlequeuedepth,
@@ -3520,6 +3524,10 @@ static int megasas_init_fw(struct megasas_instance *instance)
                         instance->msix_vectors = (readl(&instance->reg_set->
                                                         outbound_scratch_pad_2
                                                           ) & 0x1F) + 1;
+                        if (msix_vectors)
+                                instance->msix_vectors =
+                                        min(msix_vectors,
+                                            instance->msix_vectors);
                 } else
                         instance->msix_vectors = 1;
                 /* Don't bother allocating more MSI-X vectors than cpus */
@@ -5233,7 +5241,6 @@ megasas_aen_polling(struct work_struct *work)
 
                 case MR_EVT_PD_REMOVED:
                         if (megasas_get_pd_list(instance) == 0) {
-                        megasas_get_pd_list(instance);
                         for (i = 0; i < MEGASAS_MAX_PD_CHANNELS; i++) {
                                 for (j = 0;
                                 j < MEGASAS_MAX_DEV_PER_CHANNEL;
diff --git a/drivers/scsi/megaraid/megaraid_sas_fp.c b/drivers/scsi/megaraid/megaraid_sas_fp.c
index e3d251a2e26a..a11df82474ef 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fp.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fp.c
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2009-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.c b/drivers/scsi/megaraid/megaraid_sas_fusion.c
index ddf094e7d0ac..74030aff69ad 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.c
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2009-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -1184,8 +1184,6 @@ megasas_set_pd_lba(struct MPI2_RAID_SCSI_IO_REQUEST *io_request, u8 cdb_len,
                 io_request->CDB.EEDP32.PrimaryReferenceTag =
                         cpu_to_be32(ref_tag);
                 io_request->CDB.EEDP32.PrimaryApplicationTagMask = 0xffff;
-
-                io_request->DataLength = num_blocks * 512;
                 io_request->IoFlags = 32; /* Specify 32-byte cdb */
 
                 /* Transfer length */
@@ -1329,7 +1327,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
                           struct megasas_cmd_fusion *cmd)
 {
         u8 fp_possible;
-        u32 start_lba_lo, start_lba_hi, device_id;
+        u32 start_lba_lo, start_lba_hi, device_id, datalength = 0;
         struct MPI2_RAID_SCSI_IO_REQUEST *io_request;
         union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc;
         struct IO_REQUEST_INFO io_info;
@@ -1355,7 +1353,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
          * 6-byte READ(0x08) or WRITE(0x0A) cdb
          */
         if (scp->cmd_len == 6) {
-                io_request->DataLength = (u32) scp->cmnd[4];
+                datalength = (u32) scp->cmnd[4];
                 start_lba_lo = ((u32) scp->cmnd[1] << 16) |
                         ((u32) scp->cmnd[2] << 8) | (u32) scp->cmnd[3];
 
@@ -1366,7 +1364,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
          * 10-byte READ(0x28) or WRITE(0x2A) cdb
          */
         else if (scp->cmd_len == 10) {
-                io_request->DataLength = (u32) scp->cmnd[8] |
+                datalength = (u32) scp->cmnd[8] |
                         ((u32) scp->cmnd[7] << 8);
                 start_lba_lo = ((u32) scp->cmnd[2] << 24) |
                         ((u32) scp->cmnd[3] << 16) |
@@ -1377,7 +1375,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
          * 12-byte READ(0xA8) or WRITE(0xAA) cdb
          */
         else if (scp->cmd_len == 12) {
-                io_request->DataLength = ((u32) scp->cmnd[6] << 24) |
+                datalength = ((u32) scp->cmnd[6] << 24) |
                         ((u32) scp->cmnd[7] << 16) |
                         ((u32) scp->cmnd[8] << 8) | (u32) scp->cmnd[9];
                 start_lba_lo = ((u32) scp->cmnd[2] << 24) |
@@ -1389,7 +1387,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
          * 16-byte READ(0x88) or WRITE(0x8A) cdb
          */
         else if (scp->cmd_len == 16) {
-                io_request->DataLength = ((u32) scp->cmnd[10] << 24) |
+                datalength = ((u32) scp->cmnd[10] << 24) |
                         ((u32) scp->cmnd[11] << 16) |
                         ((u32) scp->cmnd[12] << 8) | (u32) scp->cmnd[13];
                 start_lba_lo = ((u32) scp->cmnd[6] << 24) |
@@ -1403,8 +1401,9 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 
         memset(&io_info, 0, sizeof(struct IO_REQUEST_INFO));
         io_info.ldStartBlock = ((u64)start_lba_hi << 32) | start_lba_lo;
-        io_info.numBlocks = io_request->DataLength;
+        io_info.numBlocks = datalength;
         io_info.ldTgtId = device_id;
+        io_request->DataLength = scsi_bufflen(scp);
 
         if (scp->sc_data_direction == PCI_DMA_FROMDEVICE)
                 io_info.isRead = 1;
@@ -1431,7 +1430,6 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
         if (fp_possible) {
                 megasas_set_pd_lba(io_request, scp->cmd_len, &io_info, scp,
                                    local_map_ptr, start_lba_lo);
-                io_request->DataLength = scsi_bufflen(scp);
                 io_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
                 cmd->request_desc->SCSIIO.RequestFlags =
                         (MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY
@@ -1510,7 +1508,8 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
         local_map_ptr = fusion->ld_map[(instance->map_id & 1)];
 
         /* Check if this is a system PD I/O */
-        if (instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) {
+        if (scmd->device->channel < MEGASAS_MAX_PD_CHANNELS &&
+            instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) {
                 io_request->Function = 0;
                 io_request->DevHandle =
                         local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
@@ -1525,6 +1524,8 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
                 cmd->request_desc->SCSIIO.RequestFlags =
                         (MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY <<
                          MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+                cmd->request_desc->SCSIIO.DevHandle =
+                        local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
         } else {
                 io_request->Function  = MEGASAS_MPI2_FUNCTION_LD_IO_REQUEST;
                 io_request->DevHandle = device_id;
@@ -1732,8 +1733,6 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
         if (reply_descript_type == MPI2_RPY_DESCRIPT_FLAGS_UNUSED)
                 return IRQ_NONE;
 
-        d_val.word = desc->Words;
-
         num_completed = 0;
 
         while ((d_val.u.low != UINT_MAX) && (d_val.u.high != UINT_MAX)) {
@@ -1855,10 +1854,8 @@ megasas_complete_cmd_dpc_fusion(unsigned long instance_addr)
         }
         spin_unlock_irqrestore(&instance->hba_lock, flags);
 
-        spin_lock_irqsave(&instance->completion_lock, flags);
         for (MSIxIndex = 0 ; MSIxIndex < count; MSIxIndex++)
                 complete_cmd_fusion(instance, MSIxIndex);
-        spin_unlock_irqrestore(&instance->completion_lock, flags);
 }
 
 /**
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.h b/drivers/scsi/megaraid/megaraid_sas_fusion.h
index 088c9f91da95..a7c64f051996 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.h
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.h
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2009-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/scsi/mvumi.c b/drivers/scsi/mvumi.c
index 783edc7c6b98..c585a925b3cd 100644
--- a/drivers/scsi/mvumi.c
+++ b/drivers/scsi/mvumi.c
@@ -35,10 +35,12 @@
 #include <linux/io.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_transport.h>
 #include <scsi/scsi_eh.h>
 #include <linux/uaccess.h>
+#include <linux/kthread.h>
 
 #include "mvumi.h"
 
@@ -48,6 +50,7 @@ MODULE_DESCRIPTION("Marvell UMI Driver");
 
 static DEFINE_PCI_DEVICE_TABLE(mvumi_pci_table) = {
         { PCI_DEVICE(PCI_VENDOR_ID_MARVELL_2, PCI_DEVICE_ID_MARVELL_MV9143) },
+        { PCI_DEVICE(PCI_VENDOR_ID_MARVELL_2, PCI_DEVICE_ID_MARVELL_MV9580) },
         { 0 }
 };
 
@@ -118,7 +121,7 @@ static int mvumi_map_pci_addr(struct pci_dev *dev, void **addr_array)
 static struct mvumi_res *mvumi_alloc_mem_resource(struct mvumi_hba *mhba,
                                 enum resource_type type, unsigned int size)
 {
-        struct mvumi_res *res = kzalloc(sizeof(*res), GFP_KERNEL);
+        struct mvumi_res *res = kzalloc(sizeof(*res), GFP_ATOMIC);
 
         if (!res) {
                 dev_err(&mhba->pdev->dev,
@@ -128,7 +131,7 @@ static struct mvumi_res *mvumi_alloc_mem_resource(struct mvumi_hba *mhba,
 
         switch (type) {
         case RESOURCE_CACHED_MEMORY:
-                res->virt_addr = kzalloc(size, GFP_KERNEL);
+                res->virt_addr = kzalloc(size, GFP_ATOMIC);
                 if (!res->virt_addr) {
                         dev_err(&mhba->pdev->dev,
                                 "unable to allocate memory,size = %d.\n", size);
@@ -222,11 +225,11 @@ static int mvumi_make_sgl(struct mvumi_hba *mhba, struct scsi_cmnd *scmd,
                         m_sg->baseaddr_l = cpu_to_le32(lower_32_bits(busaddr));
                         m_sg->baseaddr_h = cpu_to_le32(upper_32_bits(busaddr));
                         m_sg->flags = 0;
-                        m_sg->size = cpu_to_le32(sg_dma_len(&sg[i]));
+                        sgd_setsz(mhba, m_sg, cpu_to_le32(sg_dma_len(&sg[i])));
                         if ((i + 1) == *sg_count)
-                                m_sg->flags |= SGD_EOT;
+                                m_sg->flags |= 1U << mhba->eot_flag;
 
-                        m_sg++;
+                        sgd_inc(mhba, m_sg);
                 }
         } else {
                 scmd->SCp.dma_handle = scsi_bufflen(scmd) ?
@@ -237,8 +240,8 @@ static int mvumi_make_sgl(struct mvumi_hba *mhba, struct scsi_cmnd *scmd,
                 busaddr = scmd->SCp.dma_handle;
                 m_sg->baseaddr_l = cpu_to_le32(lower_32_bits(busaddr));
                 m_sg->baseaddr_h = cpu_to_le32(upper_32_bits(busaddr));
-                m_sg->flags = SGD_EOT;
-                m_sg->size = cpu_to_le32(scsi_bufflen(scmd));
+                m_sg->flags = 1U << mhba->eot_flag;
+                sgd_setsz(mhba, m_sg, cpu_to_le32(scsi_bufflen(scmd)));
                 *sg_count = 1;
         }
 
@@ -267,8 +270,8 @@ static int mvumi_internal_cmd_sgl(struct mvumi_hba *mhba, struct mvumi_cmd *cmd,
 
         m_sg->baseaddr_l = cpu_to_le32(lower_32_bits(phy_addr));
         m_sg->baseaddr_h = cpu_to_le32(upper_32_bits(phy_addr));
-        m_sg->flags = SGD_EOT;
-        m_sg->size = cpu_to_le32(size);
+        m_sg->flags = 1U << mhba->eot_flag;
+        sgd_setsz(mhba, m_sg, cpu_to_le32(size));
 
         return 0;
 }
@@ -285,7 +288,8 @@ static struct mvumi_cmd *mvumi_create_internal_cmd(struct mvumi_hba *mhba,
         }
         INIT_LIST_HEAD(&cmd->queue_pointer);
 
-        cmd->frame = kzalloc(mhba->ib_max_size, GFP_KERNEL);
+        cmd->frame = pci_alloc_consistent(mhba->pdev,
+                                mhba->ib_max_size, &cmd->frame_phys);
         if (!cmd->frame) {
                 dev_err(&mhba->pdev->dev, "failed to allocate memory for FW"
                         " frame,size = %d.\n", mhba->ib_max_size);
@@ -297,7 +301,8 @@ static struct mvumi_cmd *mvumi_create_internal_cmd(struct mvumi_hba *mhba,
                 if (mvumi_internal_cmd_sgl(mhba, cmd, buf_size)) {
                         dev_err(&mhba->pdev->dev, "failed to allocate memory"
                                                 " for internal frame\n");
-                        kfree(cmd->frame);
+                        pci_free_consistent(mhba->pdev, mhba->ib_max_size,
+                                        cmd->frame, cmd->frame_phys);
                         kfree(cmd);
                         return NULL;
                 }
@@ -317,7 +322,7 @@ static void mvumi_delete_internal_cmd(struct mvumi_hba *mhba,
         if (cmd && cmd->frame) {
                 if (cmd->frame->sg_counts) {
                         m_sg = (struct mvumi_sgl *) &cmd->frame->payload[0];
-                        size = m_sg->size;
+                        sgd_getsz(mhba, m_sg, size);
 
                         phy_addr = (dma_addr_t) m_sg->baseaddr_l |
                                 (dma_addr_t) ((m_sg->baseaddr_h << 16) << 16);
@@ -325,7 +330,8 @@ static void mvumi_delete_internal_cmd(struct mvumi_hba *mhba,
                         pci_free_consistent(mhba->pdev, size, cmd->data_buf,
                                                                 phy_addr);
                 }
-                kfree(cmd->frame);
+                pci_free_consistent(mhba->pdev, mhba->ib_max_size,
+                                cmd->frame, cmd->frame_phys);
                 kfree(cmd);
         }
 }
@@ -374,7 +380,8 @@ static void mvumi_free_cmds(struct mvumi_hba *mhba)
                 cmd = list_first_entry(&mhba->cmd_pool, struct mvumi_cmd,
                                                         queue_pointer);
                 list_del(&cmd->queue_pointer);
-                kfree(cmd->frame);
+                if (!(mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC))
+                        kfree(cmd->frame);
                 kfree(cmd);
         }
 }
@@ -396,7 +403,12 @@ static int mvumi_alloc_cmds(struct mvumi_hba *mhba)
 
                 INIT_LIST_HEAD(&cmd->queue_pointer);
                 list_add_tail(&cmd->queue_pointer, &mhba->cmd_pool);
-                cmd->frame = kzalloc(mhba->ib_max_size, GFP_KERNEL);
+                if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+                        cmd->frame = mhba->ib_frame + i * mhba->ib_max_size;
+                        cmd->frame_phys = mhba->ib_frame_phys
+                                                + i * mhba->ib_max_size;
+                } else
+                        cmd->frame = kzalloc(mhba->ib_max_size, GFP_KERNEL);
                 if (!cmd->frame)
                         goto err_exit;
         }
@@ -409,48 +421,71 @@ err_exit:
                 cmd = list_first_entry(&mhba->cmd_pool, struct mvumi_cmd,
                                                 queue_pointer);
                 list_del(&cmd->queue_pointer);
-                kfree(cmd->frame);
+                if (!(mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC))
+                        kfree(cmd->frame);
                 kfree(cmd);
         }
         return -ENOMEM;
 }
 
-static int mvumi_get_ib_list_entry(struct mvumi_hba *mhba, void **ib_entry)
+static unsigned int mvumi_check_ib_list_9143(struct mvumi_hba *mhba)
 {
-        unsigned int ib_rp_reg, cur_ib_entry;
+        unsigned int ib_rp_reg;
+        struct mvumi_hw_regs *regs = mhba->regs;
+
+        ib_rp_reg = ioread32(mhba->regs->inb_read_pointer);
 
+        if (unlikely(((ib_rp_reg & regs->cl_slot_num_mask) ==
+                        (mhba->ib_cur_slot & regs->cl_slot_num_mask)) &&
+                        ((ib_rp_reg & regs->cl_pointer_toggle)
+                         != (mhba->ib_cur_slot & regs->cl_pointer_toggle)))) {
+                dev_warn(&mhba->pdev->dev, "no free slot to use.\n");
+                return 0;
+        }
         if (atomic_read(&mhba->fw_outstanding) >= mhba->max_io) {
                 dev_warn(&mhba->pdev->dev, "firmware io overflow.\n");
-                return -1;
+                return 0;
+        } else {
+                return mhba->max_io - atomic_read(&mhba->fw_outstanding);
         }
-        ib_rp_reg = ioread32(mhba->mmio + CLA_INB_READ_POINTER);
+}
 
-        if (unlikely(((ib_rp_reg & CL_SLOT_NUM_MASK) ==
-                        (mhba->ib_cur_slot & CL_SLOT_NUM_MASK)) &&
-                        ((ib_rp_reg & CL_POINTER_TOGGLE) !=
-                        (mhba->ib_cur_slot & CL_POINTER_TOGGLE)))) {
-                dev_warn(&mhba->pdev->dev, "no free slot to use.\n");
-                return -1;
-        }
+static unsigned int mvumi_check_ib_list_9580(struct mvumi_hba *mhba)
+{
+        unsigned int count;
+        if (atomic_read(&mhba->fw_outstanding) >= (mhba->max_io - 1))
+                return 0;
+        count = ioread32(mhba->ib_shadow);
+        if (count == 0xffff)
+                return 0;
+        return count;
+}
+
+static void mvumi_get_ib_list_entry(struct mvumi_hba *mhba, void **ib_entry)
+{
+        unsigned int cur_ib_entry;
 
-        cur_ib_entry = mhba->ib_cur_slot & CL_SLOT_NUM_MASK;
+        cur_ib_entry = mhba->ib_cur_slot & mhba->regs->cl_slot_num_mask;
         cur_ib_entry++;
         if (cur_ib_entry >= mhba->list_num_io) {
                 cur_ib_entry -= mhba->list_num_io;
-                mhba->ib_cur_slot ^= CL_POINTER_TOGGLE;
+                mhba->ib_cur_slot ^= mhba->regs->cl_pointer_toggle;
+        }
+        mhba->ib_cur_slot &= ~mhba->regs->cl_slot_num_mask;
+        mhba->ib_cur_slot |= (cur_ib_entry & mhba->regs->cl_slot_num_mask);
+        if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+                *ib_entry = mhba->ib_list + cur_ib_entry *
+                                sizeof(struct mvumi_dyn_list_entry);
+        } else {
+                *ib_entry = mhba->ib_list + cur_ib_entry * mhba->ib_max_size;
         }
-        mhba->ib_cur_slot &= ~CL_SLOT_NUM_MASK;
-        mhba->ib_cur_slot |= (cur_ib_entry & CL_SLOT_NUM_MASK);
-        *ib_entry = mhba->ib_list + cur_ib_entry * mhba->ib_max_size;
         atomic_inc(&mhba->fw_outstanding);
-
-        return 0;
 }
 
 static void mvumi_send_ib_list_entry(struct mvumi_hba *mhba)
 {
-        iowrite32(0xfff, mhba->ib_shadow);
-        iowrite32(mhba->ib_cur_slot, mhba->mmio + CLA_INB_WRITE_POINTER);
+        iowrite32(0xffff, mhba->ib_shadow);
+        iowrite32(mhba->ib_cur_slot, mhba->regs->inb_write_pointer);
 }
 
 static char mvumi_check_ob_frame(struct mvumi_hba *mhba,
@@ -480,31 +515,59 @@ static char mvumi_check_ob_frame(struct mvumi_hba *mhba,
         return 0;
 }
 
-static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
+static int mvumi_check_ob_list_9143(struct mvumi_hba *mhba,
+                        unsigned int *cur_obf, unsigned int *assign_obf_end)
 {
-        unsigned int ob_write_reg, ob_write_shadow_reg;
-        unsigned int cur_obf, assign_obf_end, i;
-        struct mvumi_ob_data *ob_data;
-        struct mvumi_rsp_frame *p_outb_frame;
+        unsigned int ob_write, ob_write_shadow;
+        struct mvumi_hw_regs *regs = mhba->regs;
 
         do {
-                ob_write_reg = ioread32(mhba->mmio + CLA_OUTB_COPY_POINTER);
-                ob_write_shadow_reg = ioread32(mhba->ob_shadow);
-        } while ((ob_write_reg & CL_SLOT_NUM_MASK) != ob_write_shadow_reg);
+                ob_write = ioread32(regs->outb_copy_pointer);
+                ob_write_shadow = ioread32(mhba->ob_shadow);
+        } while ((ob_write & regs->cl_slot_num_mask) != ob_write_shadow);
 
-        cur_obf = mhba->ob_cur_slot & CL_SLOT_NUM_MASK;
-        assign_obf_end = ob_write_reg & CL_SLOT_NUM_MASK;
+        *cur_obf = mhba->ob_cur_slot & mhba->regs->cl_slot_num_mask;
+        *assign_obf_end = ob_write & mhba->regs->cl_slot_num_mask;
 
-        if ((ob_write_reg & CL_POINTER_TOGGLE) !=
-                                (mhba->ob_cur_slot & CL_POINTER_TOGGLE)) {
-                assign_obf_end += mhba->list_num_io;
+        if ((ob_write & regs->cl_pointer_toggle) !=
+                        (mhba->ob_cur_slot & regs->cl_pointer_toggle)) {
+                *assign_obf_end += mhba->list_num_io;
         }
+        return 0;
+}
+
+static int mvumi_check_ob_list_9580(struct mvumi_hba *mhba,
+                        unsigned int *cur_obf, unsigned int *assign_obf_end)
+{
+        unsigned int ob_write;
+        struct mvumi_hw_regs *regs = mhba->regs;
+
+        ob_write = ioread32(regs->outb_read_pointer);
+        ob_write = ioread32(regs->outb_copy_pointer);
+        *cur_obf = mhba->ob_cur_slot & mhba->regs->cl_slot_num_mask;
+        *assign_obf_end = ob_write & mhba->regs->cl_slot_num_mask;
+        if (*assign_obf_end < *cur_obf)
+                *assign_obf_end += mhba->list_num_io;
+        else if (*assign_obf_end == *cur_obf)
+                return -1;
+        return 0;
+}
+
+static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
+{
+        unsigned int cur_obf, assign_obf_end, i;
+        struct mvumi_ob_data *ob_data;
+        struct mvumi_rsp_frame *p_outb_frame;
+        struct mvumi_hw_regs *regs = mhba->regs;
+
+        if (mhba->instancet->check_ob_list(mhba, &cur_obf, &assign_obf_end))
+                return;
 
         for (i = (assign_obf_end - cur_obf); i != 0; i--) {
                 cur_obf++;
                 if (cur_obf >= mhba->list_num_io) {
                         cur_obf -= mhba->list_num_io;
-                        mhba->ob_cur_slot ^= CL_POINTER_TOGGLE;
+                        mhba->ob_cur_slot ^= regs->cl_pointer_toggle;
                 }
 
                 p_outb_frame = mhba->ob_list + cur_obf * mhba->ob_max_size;
@@ -528,7 +591,7 @@ static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
                         ob_data = NULL;
                         if (cur_obf == 0) {
                                 cur_obf = mhba->list_num_io - 1;
-                                mhba->ob_cur_slot ^= CL_POINTER_TOGGLE;
+                                mhba->ob_cur_slot ^= regs->cl_pointer_toggle;
                         } else
                                 cur_obf -= 1;
                         break;
@@ -539,18 +602,20 @@ static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
 
                 list_add_tail(&ob_data->list, &mhba->free_ob_list);
         }
-        mhba->ob_cur_slot &= ~CL_SLOT_NUM_MASK;
-        mhba->ob_cur_slot |= (cur_obf & CL_SLOT_NUM_MASK);
-        iowrite32(mhba->ob_cur_slot, mhba->mmio + CLA_OUTB_READ_POINTER);
+        mhba->ob_cur_slot &= ~regs->cl_slot_num_mask;
+        mhba->ob_cur_slot |= (cur_obf & regs->cl_slot_num_mask);
+        iowrite32(mhba->ob_cur_slot, regs->outb_read_pointer);
 }
 
-static void mvumi_reset(void *regs)
+static void mvumi_reset(struct mvumi_hba *mhba)
 {
-        iowrite32(0, regs + CPU_ENPOINTA_MASK_REG);
-        if (ioread32(regs + CPU_ARM_TO_PCIEA_MSG1) != HANDSHAKE_DONESTATE)
+        struct mvumi_hw_regs *regs = mhba->regs;
+
+        iowrite32(0, regs->enpointa_mask_reg);
+        if (ioread32(regs->arm_to_pciea_msg1) != HANDSHAKE_DONESTATE)
                 return;
 
-        iowrite32(DRBL_SOFT_RESET, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+        iowrite32(DRBL_SOFT_RESET, regs->pciea_to_arm_drbl_reg);
 }
 
 static unsigned char mvumi_start(struct mvumi_hba *mhba);
@@ -558,7 +623,7 @@ static unsigned char mvumi_start(struct mvumi_hba *mhba);
 static int mvumi_wait_for_outstanding(struct mvumi_hba *mhba)
 {
         mhba->fw_state = FW_STATE_ABORT;
-        mvumi_reset(mhba->mmio);
+        mvumi_reset(mhba);
 
         if (mvumi_start(mhba))
                 return FAILED;
@@ -566,6 +631,98 @@ static int mvumi_wait_for_outstanding(struct mvumi_hba *mhba)
                 return SUCCESS;
 }
 
+static int mvumi_wait_for_fw(struct mvumi_hba *mhba)
+{
+        struct mvumi_hw_regs *regs = mhba->regs;
+        u32 tmp;
+        unsigned long before;
+        before = jiffies;
+
+        iowrite32(0, regs->enpointa_mask_reg);
+        tmp = ioread32(regs->arm_to_pciea_msg1);
+        while (tmp != HANDSHAKE_READYSTATE) {
+                iowrite32(DRBL_MU_RESET, regs->pciea_to_arm_drbl_reg);
+                if (time_after(jiffies, before + FW_MAX_DELAY * HZ)) {
+                        dev_err(&mhba->pdev->dev,
+                                "FW reset failed [0x%x].\n", tmp);
+                        return FAILED;
+                }
+
+                msleep(500);
+                rmb();
+                tmp = ioread32(regs->arm_to_pciea_msg1);
+        }
+
+        return SUCCESS;
+}
+
+static void mvumi_backup_bar_addr(struct mvumi_hba *mhba)
+{
+        unsigned char i;
+
+        for (i = 0; i < MAX_BASE_ADDRESS; i++) {
+                pci_read_config_dword(mhba->pdev, 0x10 + i * 4,
+                                                &mhba->pci_base[i]);
+        }
+}
+
+static void mvumi_restore_bar_addr(struct mvumi_hba *mhba)
+{
+        unsigned char i;
+
+        for (i = 0; i < MAX_BASE_ADDRESS; i++) {
+                if (mhba->pci_base[i])
+                        pci_write_config_dword(mhba->pdev, 0x10 + i * 4,
+                                                mhba->pci_base[i]);
+        }
+}
+
+static unsigned int mvumi_pci_set_master(struct pci_dev *pdev)
+{
+        unsigned int ret = 0;
+        pci_set_master(pdev);
+
+        if (IS_DMA64) {
+                if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64)))
+                        ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+        } else
+                ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+
+        return ret;
+}
+
+static int mvumi_reset_host_9580(struct mvumi_hba *mhba)
+{
+        mhba->fw_state = FW_STATE_ABORT;
+
+        iowrite32(0, mhba->regs->reset_enable);
+        iowrite32(0xf, mhba->regs->reset_request);
+
+        iowrite32(0x10, mhba->regs->reset_enable);
+        iowrite32(0x10, mhba->regs->reset_request);
+        msleep(100);
+        pci_disable_device(mhba->pdev);
+
+        if (pci_enable_device(mhba->pdev)) {
+                dev_err(&mhba->pdev->dev, "enable device failed\n");
+                return FAILED;
+        }
+        if (mvumi_pci_set_master(mhba->pdev)) {
+                dev_err(&mhba->pdev->dev, "set master failed\n");
+                return FAILED;
+        }
+        mvumi_restore_bar_addr(mhba);
+        if (mvumi_wait_for_fw(mhba) == FAILED)
+                return FAILED;
+
+        return mvumi_wait_for_outstanding(mhba);
+}
+
+static int mvumi_reset_host_9143(struct mvumi_hba *mhba)
+{
+        return mvumi_wait_for_outstanding(mhba);
+}
+
 static int mvumi_host_reset(struct scsi_cmnd *scmd)
 {
         struct mvumi_hba *mhba;
@@ -575,7 +732,7 @@ static int mvumi_host_reset(struct scsi_cmnd *scmd)
         scmd_printk(KERN_NOTICE, scmd, "RESET -%ld cmd=%x retries=%x\n",
                         scmd->serial_number, scmd->cmnd[0], scmd->retries);
 
-        return mvumi_wait_for_outstanding(mhba);
+        return mhba->instancet->reset_host(mhba);
 }
 
 static int mvumi_issue_blocked_cmd(struct mvumi_hba *mhba,
@@ -628,7 +785,9 @@ static void mvumi_release_fw(struct mvumi_hba *mhba)
         mvumi_free_cmds(mhba);
         mvumi_release_mem_resource(mhba);
         mvumi_unmap_pci_addr(mhba->pdev, mhba->base_addr);
-        kfree(mhba->handshake_page);
+        pci_free_consistent(mhba->pdev, HSP_MAX_SIZE,
+                mhba->handshake_page, mhba->handshake_page_phys);
+        kfree(mhba->regs);
         pci_release_regions(mhba->pdev);
 }
 
@@ -665,6 +824,7 @@ get_cmd:	cmd = mvumi_create_internal_cmd(mhba, 0);
                 frame->cdb_length = MAX_COMMAND_SIZE;
                 memset(frame->cdb, 0, MAX_COMMAND_SIZE);
                 frame->cdb[0] = SCSI_CMD_MARVELL_SPECIFIC;
+                frame->cdb[1] = CDB_CORE_MODULE;
                 frame->cdb[2] = CDB_CORE_SHUTDOWN;
 
                 mvumi_issue_blocked_cmd(mhba, cmd);
@@ -695,7 +855,7 @@ mvumi_calculate_checksum(struct mvumi_hs_header *p_header,
         return ret;
 }
 
-void mvumi_hs_build_page(struct mvumi_hba *mhba,
+static void mvumi_hs_build_page(struct mvumi_hba *mhba,
                                 struct mvumi_hs_header *hs_header)
 {
         struct mvumi_hs_page2 *hs_page2;
@@ -710,6 +870,8 @@ void mvumi_hs_build_page(struct mvumi_hba *mhba,
                 hs_header->frame_length = sizeof(*hs_page2) - 4;
                 memset(hs_header->frame_content, 0, hs_header->frame_length);
                 hs_page2->host_type = 3; /* 3 mean linux*/
+                if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC)
+                        hs_page2->host_cap = 0x08;/* host dynamic source mode */
                 hs_page2->host_ver.ver_major = VER_MAJOR;
                 hs_page2->host_ver.ver_minor = VER_MINOR;
                 hs_page2->host_ver.ver_oem = VER_OEM;
@@ -745,8 +907,18 @@ void mvumi_hs_build_page(struct mvumi_hba *mhba,
                 hs_page4->ob_baseaddr_h = upper_32_bits(mhba->ob_list_phys);
                 hs_page4->ib_entry_size = mhba->ib_max_size_setting;
                 hs_page4->ob_entry_size = mhba->ob_max_size_setting;
-                hs_page4->ob_depth = mhba->list_num_io;
-                hs_page4->ib_depth = mhba->list_num_io;
+                if (mhba->hba_capability
+                        & HS_CAPABILITY_NEW_PAGE_IO_DEPTH_DEF) {
+                        hs_page4->ob_depth = find_first_bit((unsigned long *)
+                                                            &mhba->list_num_io,
+                                                            BITS_PER_LONG);
+                        hs_page4->ib_depth = find_first_bit((unsigned long *)
+                                                            &mhba->list_num_io,
+                                                            BITS_PER_LONG);
+                } else {
+                        hs_page4->ob_depth = (u8) mhba->list_num_io;
+                        hs_page4->ib_depth = (u8) mhba->list_num_io;
+                }
                 hs_header->checksum = mvumi_calculate_checksum(hs_header,
                                                 hs_header->frame_length);
                 break;
@@ -774,8 +946,11 @@ static int mvumi_init_data(struct mvumi_hba *mhba)
                 return 0;
 
         tmp_size = mhba->ib_max_size * mhba->max_io;
+        if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC)
+                tmp_size += sizeof(struct mvumi_dyn_list_entry) * mhba->max_io;
+
         tmp_size += 128 + mhba->ob_max_size * mhba->max_io;
-        tmp_size += 8 + sizeof(u32) + 16;
+        tmp_size += 8 + sizeof(u32)*2 + 16;
 
         res_mgnt = mvumi_alloc_mem_resource(mhba,
                                         RESOURCE_UNCACHED_MEMORY, tmp_size);
@@ -793,24 +968,41 @@ static int mvumi_init_data(struct mvumi_hba *mhba)
         v += offset;
         mhba->ib_list = v;
         mhba->ib_list_phys = p;
+        if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+                v += sizeof(struct mvumi_dyn_list_entry) * mhba->max_io;
+                p += sizeof(struct mvumi_dyn_list_entry) * mhba->max_io;
+                mhba->ib_frame = v;
+                mhba->ib_frame_phys = p;
+        }
         v += mhba->ib_max_size * mhba->max_io;
         p += mhba->ib_max_size * mhba->max_io;
+
         /* ib shadow */
         offset = round_up(p, 8) - p;
         p += offset;
         v += offset;
         mhba->ib_shadow = v;
         mhba->ib_shadow_phys = p;
-        p += sizeof(u32);
-        v += sizeof(u32);
+        p += sizeof(u32)*2;
+        v += sizeof(u32)*2;
         /* ob shadow */
-        offset = round_up(p, 8) - p;
-        p += offset;
-        v += offset;
-        mhba->ob_shadow = v;
-        mhba->ob_shadow_phys = p;
-        p += 8;
-        v += 8;
+        if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580) {
+                offset = round_up(p, 8) - p;
+                p += offset;
+                v += offset;
+                mhba->ob_shadow = v;
+                mhba->ob_shadow_phys = p;
+                p += 8;
+                v += 8;
+        } else {
+                offset = round_up(p, 4) - p;
+                p += offset;
+                v += offset;
+                mhba->ob_shadow = v;
+                mhba->ob_shadow_phys = p;
+                p += 4;
+                v += 4;
+        }
 
         /* ob list */
         offset = round_up(p, 128) - p;
@@ -902,6 +1094,12 @@ static int mvumi_hs_process_page(struct mvumi_hba *mhba,
                 dev_dbg(&mhba->pdev->dev, "FW version:%d\n",
                                                 hs_page1->fw_ver.ver_build);
 
+                if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)
+                        mhba->eot_flag = 22;
+                else
+                        mhba->eot_flag = 27;
+                if (mhba->hba_capability & HS_CAPABILITY_NEW_PAGE_IO_DEPTH_DEF)
+                        mhba->list_num_io = 1 << hs_page1->cl_inout_list_depth;
                 break;
         default:
                 dev_err(&mhba->pdev->dev, "handshake: page code error\n");
@@ -923,12 +1121,12 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
 {
         unsigned int hs_state, tmp, hs_fun;
         struct mvumi_hs_header *hs_header;
-        void *regs = mhba->mmio;
+        struct mvumi_hw_regs *regs = mhba->regs;
 
         if (mhba->fw_state == FW_STATE_STARTING)
                 hs_state = HS_S_START;
         else {
-                tmp = ioread32(regs + CPU_ARM_TO_PCIEA_MSG0);
+                tmp = ioread32(regs->arm_to_pciea_msg0);
                 hs_state = HS_GET_STATE(tmp);
                 dev_dbg(&mhba->pdev->dev, "handshake state[0x%x].\n", hs_state);
                 if (HS_GET_STATUS(tmp) != HS_STATUS_OK) {
@@ -943,21 +1141,20 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
                 mhba->fw_state = FW_STATE_HANDSHAKING;
                 HS_SET_STATUS(hs_fun, HS_STATUS_OK);
                 HS_SET_STATE(hs_fun, HS_S_RESET);
-                iowrite32(HANDSHAKE_SIGNATURE, regs + CPU_PCIEA_TO_ARM_MSG1);
-                iowrite32(hs_fun, regs + CPU_PCIEA_TO_ARM_MSG0);
-                iowrite32(DRBL_HANDSHAKE, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+                iowrite32(HANDSHAKE_SIGNATURE, regs->pciea_to_arm_msg1);
+                iowrite32(hs_fun, regs->pciea_to_arm_msg0);
+                iowrite32(DRBL_HANDSHAKE, regs->pciea_to_arm_drbl_reg);
                 break;
 
         case HS_S_RESET:
                 iowrite32(lower_32_bits(mhba->handshake_page_phys),
-                                        regs + CPU_PCIEA_TO_ARM_MSG1);
+                                        regs->pciea_to_arm_msg1);
                 iowrite32(upper_32_bits(mhba->handshake_page_phys),
-                                        regs + CPU_ARM_TO_PCIEA_MSG1);
+                                        regs->arm_to_pciea_msg1);
                 HS_SET_STATUS(hs_fun, HS_STATUS_OK);
                 HS_SET_STATE(hs_fun, HS_S_PAGE_ADDR);
-                iowrite32(hs_fun, regs + CPU_PCIEA_TO_ARM_MSG0);
-                iowrite32(DRBL_HANDSHAKE, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
-
+                iowrite32(hs_fun, regs->pciea_to_arm_msg0);
+                iowrite32(DRBL_HANDSHAKE, regs->pciea_to_arm_drbl_reg);
                 break;
 
         case HS_S_PAGE_ADDR:
@@ -997,30 +1194,37 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
                         HS_SET_STATE(hs_fun, HS_S_END);
 
                 HS_SET_STATUS(hs_fun, HS_STATUS_OK);
-                iowrite32(hs_fun, regs + CPU_PCIEA_TO_ARM_MSG0);
-                iowrite32(DRBL_HANDSHAKE, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+                iowrite32(hs_fun, regs->pciea_to_arm_msg0);
+                iowrite32(DRBL_HANDSHAKE, regs->pciea_to_arm_drbl_reg);
                 break;
 
         case HS_S_END:
                 /* Set communication list ISR */
-                tmp = ioread32(regs + CPU_ENPOINTA_MASK_REG);
-                tmp |= INT_MAP_COMAOUT | INT_MAP_COMAERR;
-                iowrite32(tmp, regs + CPU_ENPOINTA_MASK_REG);
+                tmp = ioread32(regs->enpointa_mask_reg);
+                tmp |= regs->int_comaout | regs->int_comaerr;
+                iowrite32(tmp, regs->enpointa_mask_reg);
                 iowrite32(mhba->list_num_io, mhba->ib_shadow);
                 /* Set InBound List Available count shadow */
                 iowrite32(lower_32_bits(mhba->ib_shadow_phys),
-                                        regs + CLA_INB_AVAL_COUNT_BASEL);
+                                        regs->inb_aval_count_basel);
                 iowrite32(upper_32_bits(mhba->ib_shadow_phys),
-                                        regs + CLA_INB_AVAL_COUNT_BASEH);
-
-                /* Set OutBound List Available count shadow */
-                iowrite32((mhba->list_num_io-1) | CL_POINTER_TOGGLE,
-                                                mhba->ob_shadow);
-                iowrite32(lower_32_bits(mhba->ob_shadow_phys), regs + 0x5B0);
-                iowrite32(upper_32_bits(mhba->ob_shadow_phys), regs + 0x5B4);
+                                        regs->inb_aval_count_baseh);
+
+                if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9143) {
+                        /* Set OutBound List Available count shadow */
+                        iowrite32((mhba->list_num_io-1) |
+                                                        regs->cl_pointer_toggle,
+                                                        mhba->ob_shadow);
+                        iowrite32(lower_32_bits(mhba->ob_shadow_phys),
+                                                        regs->outb_copy_basel);
+                        iowrite32(upper_32_bits(mhba->ob_shadow_phys),
+                                                        regs->outb_copy_baseh);
+                }
 
-                mhba->ib_cur_slot = (mhba->list_num_io - 1) | CL_POINTER_TOGGLE;
-                mhba->ob_cur_slot = (mhba->list_num_io - 1) | CL_POINTER_TOGGLE;
+                mhba->ib_cur_slot = (mhba->list_num_io - 1) |
+                                                        regs->cl_pointer_toggle;
+                mhba->ob_cur_slot = (mhba->list_num_io - 1) |
+                                                        regs->cl_pointer_toggle;
                 mhba->fw_state = FW_STATE_STARTED;
 
                 break;
@@ -1040,7 +1244,7 @@ static unsigned char mvumi_handshake_event(struct mvumi_hba *mhba)
         before = jiffies;
         mvumi_handshake(mhba);
         do {
-                isr_status = mhba->instancet->read_fw_status_reg(mhba->mmio);
+                isr_status = mhba->instancet->read_fw_status_reg(mhba);
 
                 if (mhba->fw_state == FW_STATE_STARTED)
                         return 0;
@@ -1062,16 +1266,15 @@ static unsigned char mvumi_handshake_event(struct mvumi_hba *mhba)
 
 static unsigned char mvumi_check_handshake(struct mvumi_hba *mhba)
 {
-        void *regs = mhba->mmio;
         unsigned int tmp;
         unsigned long before;
 
         before = jiffies;
-        tmp = ioread32(regs + CPU_ARM_TO_PCIEA_MSG1);
+        tmp = ioread32(mhba->regs->arm_to_pciea_msg1);
         while ((tmp != HANDSHAKE_READYSTATE) && (tmp != HANDSHAKE_DONESTATE)) {
                 if (tmp != HANDSHAKE_READYSTATE)
                         iowrite32(DRBL_MU_RESET,
-                                        regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+                                        mhba->regs->pciea_to_arm_drbl_reg);
                 if (time_after(jiffies, before + FW_MAX_DELAY * HZ)) {
                         dev_err(&mhba->pdev->dev,
                                 "invalid signature [0x%x].\n", tmp);
@@ -1079,7 +1282,7 @@ static unsigned char mvumi_check_handshake(struct mvumi_hba *mhba)
                 }
                 usleep_range(1000, 2000);
                 rmb();
-                tmp = ioread32(regs + CPU_ARM_TO_PCIEA_MSG1);
+                tmp = ioread32(mhba->regs->arm_to_pciea_msg1);
         }
 
         mhba->fw_state = FW_STATE_STARTING;
@@ -1100,15 +1303,17 @@ static unsigned char mvumi_check_handshake(struct mvumi_hba *mhba)
 
 static unsigned char mvumi_start(struct mvumi_hba *mhba)
 {
-        void *regs = mhba->mmio;
         unsigned int tmp;
+        struct mvumi_hw_regs *regs = mhba->regs;
+
         /* clear Door bell */
-        tmp = ioread32(regs + CPU_ARM_TO_PCIEA_DRBL_REG);
-        iowrite32(tmp, regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+        tmp = ioread32(regs->arm_to_pciea_drbl_reg);
+        iowrite32(tmp, regs->arm_to_pciea_drbl_reg);
 
-        iowrite32(0x3FFFFFFF, regs + CPU_ARM_TO_PCIEA_MASK_REG);
-        tmp = ioread32(regs + CPU_ENPOINTA_MASK_REG) | INT_MAP_DL_CPU2PCIEA;
-        iowrite32(tmp, regs + CPU_ENPOINTA_MASK_REG);
+        iowrite32(regs->int_drbl_int_mask, regs->arm_to_pciea_mask_reg);
+        tmp = ioread32(regs->enpointa_mask_reg) | regs->int_dl_cpu2pciea;
+        iowrite32(tmp, regs->enpointa_mask_reg);
+        msleep(100);
         if (mvumi_check_handshake(mhba))
                 return -1;
 
@@ -1166,6 +1371,7 @@ static void mvumi_complete_cmd(struct mvumi_hba *mhba, struct mvumi_cmd *cmd,
         cmd->scmd->scsi_done(scmd);
         mvumi_return_cmd(mhba, cmd);
 }
+
 static void mvumi_complete_internal_cmd(struct mvumi_hba *mhba,
                                                 struct mvumi_cmd *cmd,
                                         struct mvumi_rsp_frame *ob_frame)
@@ -1210,6 +1416,304 @@ static void mvumi_show_event(struct mvumi_hba *mhba,
         }
 }
 
+static int mvumi_handle_hotplug(struct mvumi_hba *mhba, u16 devid, int status)
+{
+        struct scsi_device *sdev;
+        int ret = -1;
+
+        if (status == DEVICE_OFFLINE) {
+                sdev = scsi_device_lookup(mhba->shost, 0, devid, 0);
+                if (sdev) {
+                        dev_dbg(&mhba->pdev->dev, "remove disk %d-%d-%d.\n", 0,
+                                                                sdev->id, 0);
+                        scsi_remove_device(sdev);
+                        scsi_device_put(sdev);
+                        ret = 0;
+                } else
+                        dev_err(&mhba->pdev->dev, " no disk[%d] to remove\n",
+                                                                        devid);
+        } else if (status == DEVICE_ONLINE) {
+                sdev = scsi_device_lookup(mhba->shost, 0, devid, 0);
+                if (!sdev) {
+                        scsi_add_device(mhba->shost, 0, devid, 0);
+                        dev_dbg(&mhba->pdev->dev, " add disk %d-%d-%d.\n", 0,
+                                                                devid, 0);
+                        ret = 0;
+                } else {
+                        dev_err(&mhba->pdev->dev, " don't add disk %d-%d-%d.\n",
+                                                                0, devid, 0);
+                        scsi_device_put(sdev);
+                }
+        }
+        return ret;
+}
+
+static u64 mvumi_inquiry(struct mvumi_hba *mhba,
+        unsigned int id, struct mvumi_cmd *cmd)
+{
+        struct mvumi_msg_frame *frame;
+        u64 wwid = 0;
+        int cmd_alloc = 0;
+        int data_buf_len = 64;
+
+        if (!cmd) {
+                cmd = mvumi_create_internal_cmd(mhba, data_buf_len);
+                if (cmd)
+                        cmd_alloc = 1;
+                else
+                        return 0;
+        } else {
+                memset(cmd->data_buf, 0, data_buf_len);
+        }
+        cmd->scmd = NULL;
+        cmd->cmd_status = REQ_STATUS_PENDING;
+        atomic_set(&cmd->sync_cmd, 0);
+        frame = cmd->frame;
+        frame->device_id = (u16) id;
+        frame->cmd_flag = CMD_FLAG_DATA_IN;
+        frame->req_function = CL_FUN_SCSI_CMD;
+        frame->cdb_length = 6;
+        frame->data_transfer_length = MVUMI_INQUIRY_LENGTH;
+        memset(frame->cdb, 0, frame->cdb_length);
+        frame->cdb[0] = INQUIRY;
+        frame->cdb[4] = frame->data_transfer_length;
+
+        mvumi_issue_blocked_cmd(mhba, cmd);
+
+        if (cmd->cmd_status == SAM_STAT_GOOD) {
+                if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9143)
+                        wwid = id + 1;
+                else
+                        memcpy((void *)&wwid,
+                               (cmd->data_buf + MVUMI_INQUIRY_UUID_OFF),
+                               MVUMI_INQUIRY_UUID_LEN);
+                dev_dbg(&mhba->pdev->dev,
+                        "inquiry device(0:%d:0) wwid(%llx)\n", id, wwid);
+        } else {
+                wwid = 0;
+        }
+        if (cmd_alloc)
+                mvumi_delete_internal_cmd(mhba, cmd);
+
+        return wwid;
+}
+
+static void mvumi_detach_devices(struct mvumi_hba *mhba)
+{
+        struct mvumi_device *mv_dev = NULL , *dev_next;
+        struct scsi_device *sdev = NULL;
+
+        mutex_lock(&mhba->device_lock);
+
+        /* detach Hard Disk */
+        list_for_each_entry_safe(mv_dev, dev_next,
+                &mhba->shost_dev_list, list) {
+                mvumi_handle_hotplug(mhba, mv_dev->id, DEVICE_OFFLINE);
+                list_del_init(&mv_dev->list);
+                dev_dbg(&mhba->pdev->dev, "release device(0:%d:0) wwid(%llx)\n",
+                        mv_dev->id, mv_dev->wwid);
+                kfree(mv_dev);
+        }
+        list_for_each_entry_safe(mv_dev, dev_next, &mhba->mhba_dev_list, list) {
+                list_del_init(&mv_dev->list);
+                dev_dbg(&mhba->pdev->dev, "release device(0:%d:0) wwid(%llx)\n",
+                        mv_dev->id, mv_dev->wwid);
+                kfree(mv_dev);
+        }
+
+        /* detach virtual device */
+        if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580)
+                sdev = scsi_device_lookup(mhba->shost, 0,
+                                                mhba->max_target_id - 1, 0);
+
+        if (sdev) {
+                scsi_remove_device(sdev);
+                scsi_device_put(sdev);
+        }
+
+        mutex_unlock(&mhba->device_lock);
+}
+
+static void mvumi_rescan_devices(struct mvumi_hba *mhba, int id)
+{
+        struct scsi_device *sdev;
+
+        sdev = scsi_device_lookup(mhba->shost, 0, id, 0);
+        if (sdev) {
+                scsi_rescan_device(&sdev->sdev_gendev);
+                scsi_device_put(sdev);
+        }
+}
+
+static int mvumi_match_devices(struct mvumi_hba *mhba, int id, u64 wwid)
+{
+        struct mvumi_device *mv_dev = NULL;
+
+        list_for_each_entry(mv_dev, &mhba->shost_dev_list, list) {
+                if (mv_dev->wwid == wwid) {
+                        if (mv_dev->id != id) {
+                                dev_err(&mhba->pdev->dev,
+                                        "%s has same wwid[%llx] ,"
+                                        " but different id[%d %d]\n",
+                                        __func__, mv_dev->wwid, mv_dev->id, id);
+                                return -1;
+                        } else {
+                                if (mhba->pdev->device ==
+                                                PCI_DEVICE_ID_MARVELL_MV9143)
+                                        mvumi_rescan_devices(mhba, id);
+                                return 1;
+                        }
+                }
+        }
+        return 0;
+}
+
+static void mvumi_remove_devices(struct mvumi_hba *mhba, int id)
+{
+        struct mvumi_device *mv_dev = NULL, *dev_next;
+
+        list_for_each_entry_safe(mv_dev, dev_next,
+                                &mhba->shost_dev_list, list) {
+                if (mv_dev->id == id) {
+                        dev_dbg(&mhba->pdev->dev,
+                                "detach device(0:%d:0) wwid(%llx) from HOST\n",
+                                mv_dev->id, mv_dev->wwid);
+                        mvumi_handle_hotplug(mhba, mv_dev->id, DEVICE_OFFLINE);
+                        list_del_init(&mv_dev->list);
+                        kfree(mv_dev);
+                }
+        }
+}
+
+static int mvumi_probe_devices(struct mvumi_hba *mhba)
+{
+        int id, maxid;
+        u64 wwid = 0;
+        struct mvumi_device *mv_dev = NULL;
+        struct mvumi_cmd *cmd = NULL;
+        int found = 0;
+
+        cmd = mvumi_create_internal_cmd(mhba, 64);
+        if (!cmd)
+                return -1;
+
+        if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9143)
+                maxid = mhba->max_target_id;
+        else
+                maxid = mhba->max_target_id - 1;
+
+        for (id = 0; id < maxid; id++) {
+                wwid = mvumi_inquiry(mhba, id, cmd);
+                if (!wwid) {
+                        /* device no response, remove it */
+                        mvumi_remove_devices(mhba, id);
+                } else {
+                        /* device response, add it */
+                        found = mvumi_match_devices(mhba, id, wwid);
+                        if (!found) {
+                                mvumi_remove_devices(mhba, id);
+                                mv_dev = kzalloc(sizeof(struct mvumi_device),
+                                                                GFP_KERNEL);
+                                if (!mv_dev) {
+                                        dev_err(&mhba->pdev->dev,
+                                                "%s alloc mv_dev failed\n",
+                                                __func__);
+                                        continue;
+                                }
+                                mv_dev->id = id;
+                                mv_dev->wwid = wwid;
+                                mv_dev->sdev = NULL;
+                                INIT_LIST_HEAD(&mv_dev->list);
+                                list_add_tail(&mv_dev->list,
+                                              &mhba->mhba_dev_list);
+                                dev_dbg(&mhba->pdev->dev,
+                                        "probe a new device(0:%d:0)"
+                                        " wwid(%llx)\n", id, mv_dev->wwid);
+                        } else if (found == -1)
+                                return -1;
+                        else
+                                continue;
+                }
+        }
+
+        if (cmd)
+                mvumi_delete_internal_cmd(mhba, cmd);
+
+        return 0;
+}
+
+static int mvumi_rescan_bus(void *data)
+{
+        int ret = 0;
+        struct mvumi_hba *mhba = (struct mvumi_hba *) data;
+        struct mvumi_device *mv_dev = NULL , *dev_next;
+
+        while (!kthread_should_stop()) {
+
+                set_current_state(TASK_INTERRUPTIBLE);
+                if (!atomic_read(&mhba->pnp_count))
+                        schedule();
+                msleep(1000);
+                atomic_set(&mhba->pnp_count, 0);
+                __set_current_state(TASK_RUNNING);
+
+                mutex_lock(&mhba->device_lock);
+                ret = mvumi_probe_devices(mhba);
+                if (!ret) {
+                        list_for_each_entry_safe(mv_dev, dev_next,
+                                                 &mhba->mhba_dev_list, list) {
+                                if (mvumi_handle_hotplug(mhba, mv_dev->id,
+                                                         DEVICE_ONLINE)) {
+                                        dev_err(&mhba->pdev->dev,
+                                                "%s add device(0:%d:0) failed"
+                                                "wwid(%llx) has exist\n",
+                                                __func__,
+                                                mv_dev->id, mv_dev->wwid);
+                                        list_del_init(&mv_dev->list);
+                                        kfree(mv_dev);
+                                } else {
+                                        list_move_tail(&mv_dev->list,
+                                                       &mhba->shost_dev_list);
+                                }
+                        }
+                }
+                mutex_unlock(&mhba->device_lock);
+        }
+        return 0;
+}
+
+static void mvumi_proc_msg(struct mvumi_hba *mhba,
+                                        struct mvumi_hotplug_event *param)
+{
+        u16 size = param->size;
+        const unsigned long *ar_bitmap;
+        const unsigned long *re_bitmap;
+        int index;
+
+        if (mhba->fw_flag & MVUMI_FW_ATTACH) {
+                index = -1;
+                ar_bitmap = (const unsigned long *) param->bitmap;
+                re_bitmap = (const unsigned long *) &param->bitmap[size >> 3];
+
+                mutex_lock(&mhba->sas_discovery_mutex);
+                do {
+                        index = find_next_zero_bit(ar_bitmap, size, index + 1);
+                        if (index >= size)
+                                break;
+                        mvumi_handle_hotplug(mhba, index, DEVICE_ONLINE);
+                } while (1);
+
+                index = -1;
+                do {
+                        index = find_next_zero_bit(re_bitmap, size, index + 1);
+                        if (index >= size)
+                                break;
+                        mvumi_handle_hotplug(mhba, index, DEVICE_OFFLINE);
+                } while (1);
+                mutex_unlock(&mhba->sas_discovery_mutex);
+        }
+}
+
 static void mvumi_notification(struct mvumi_hba *mhba, u8 msg, void *buffer)
 {
         if (msg == APICDB1_EVENT_GETEVENT) {
@@ -1227,6 +1731,8 @@ static void mvumi_notification(struct mvumi_hba *mhba, u8 msg, void *buffer)
                         param = &er->events[i];
                         mvumi_show_event(mhba, param);
                 }
+        } else if (msg == APICDB1_HOST_GETEVENT) {
+                mvumi_proc_msg(mhba, buffer);
         }
 }
 
@@ -1271,17 +1777,27 @@ static void mvumi_scan_events(struct work_struct *work)
         kfree(mu_ev);
 }
 
-static void mvumi_launch_events(struct mvumi_hba *mhba, u8 msg)
+static void mvumi_launch_events(struct mvumi_hba *mhba, u32 isr_status)
 {
         struct mvumi_events_wq *mu_ev;
 
-        mu_ev = kzalloc(sizeof(*mu_ev), GFP_ATOMIC);
-        if (mu_ev) {
-                INIT_WORK(&mu_ev->work_q, mvumi_scan_events);
-                mu_ev->mhba = mhba;
-                mu_ev->event = msg;
-                mu_ev->param = NULL;
-                schedule_work(&mu_ev->work_q);
+        while (isr_status & (DRBL_BUS_CHANGE | DRBL_EVENT_NOTIFY)) {
+                if (isr_status & DRBL_BUS_CHANGE) {
+                        atomic_inc(&mhba->pnp_count);
+                        wake_up_process(mhba->dm_thread);
+                        isr_status &= ~(DRBL_BUS_CHANGE);
+                        continue;
+                }
+
+                mu_ev = kzalloc(sizeof(*mu_ev), GFP_ATOMIC);
+                if (mu_ev) {
+                        INIT_WORK(&mu_ev->work_q, mvumi_scan_events);
+                        mu_ev->mhba = mhba;
+                        mu_ev->event = APICDB1_EVENT_GETEVENT;
+                        isr_status &= ~(DRBL_EVENT_NOTIFY);
+                        mu_ev->param = NULL;
+                        schedule_work(&mu_ev->work_q);
+                }
         }
 }
 
@@ -1322,16 +1838,17 @@ static irqreturn_t mvumi_isr_handler(int irq, void *devp)
                 return IRQ_NONE;
         }
 
-        if (mhba->global_isr & INT_MAP_DL_CPU2PCIEA) {
+        if (mhba->global_isr & mhba->regs->int_dl_cpu2pciea) {
+                if (mhba->isr_status & (DRBL_BUS_CHANGE | DRBL_EVENT_NOTIFY))
+                        mvumi_launch_events(mhba, mhba->isr_status);
                 if (mhba->isr_status & DRBL_HANDSHAKE_ISR) {
                         dev_warn(&mhba->pdev->dev, "enter handshake again!\n");
                         mvumi_handshake(mhba);
                 }
-                if (mhba->isr_status & DRBL_EVENT_NOTIFY)
-                        mvumi_launch_events(mhba, APICDB1_EVENT_GETEVENT);
+
         }
 
-        if (mhba->global_isr & INT_MAP_COMAOUT)
+        if (mhba->global_isr & mhba->regs->int_comaout)
                 mvumi_receive_ob_list_entry(mhba);
 
         mhba->global_isr = 0;
@@ -1358,8 +1875,7 @@ static enum mvumi_qc_result mvumi_send_command(struct mvumi_hba *mhba,
                 dev_dbg(&mhba->pdev->dev, "no free tag.\n");
                 return MV_QUEUE_COMMAND_RESULT_NO_RESOURCE;
         }
-        if (mvumi_get_ib_list_entry(mhba, &ib_entry))
-                return MV_QUEUE_COMMAND_RESULT_NO_RESOURCE;
+        mvumi_get_ib_list_entry(mhba, &ib_entry);
 
         cmd->frame->tag = tag_get_one(mhba, &mhba->tag_pool);
         cmd->frame->request_id = mhba->io_seq++;
@@ -1367,21 +1883,35 @@ static enum mvumi_qc_result mvumi_send_command(struct mvumi_hba *mhba,
         mhba->tag_cmd[cmd->frame->tag] = cmd;
         frame_len = sizeof(*ib_frame) - 4 +
                                 ib_frame->sg_counts * sizeof(struct mvumi_sgl);
-        memcpy(ib_entry, ib_frame, frame_len);
+        if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+                struct mvumi_dyn_list_entry *dle;
+                dle = ib_entry;
+                dle->src_low_addr =
+                        cpu_to_le32(lower_32_bits(cmd->frame_phys));
+                dle->src_high_addr =
+                        cpu_to_le32(upper_32_bits(cmd->frame_phys));
+                dle->if_length = (frame_len >> 2) & 0xFFF;
+        } else {
+                memcpy(ib_entry, ib_frame, frame_len);
+        }
         return MV_QUEUE_COMMAND_RESULT_SENT;
 }
 
 static void mvumi_fire_cmd(struct mvumi_hba *mhba, struct mvumi_cmd *cmd)
 {
         unsigned short num_of_cl_sent = 0;
+        unsigned int count;
         enum mvumi_qc_result result;
 
         if (cmd)
                 list_add_tail(&cmd->queue_pointer, &mhba->waiting_req_list);
+        count = mhba->instancet->check_ib_list(mhba);
+        if (list_empty(&mhba->waiting_req_list) || !count)
+                return;
 
-        while (!list_empty(&mhba->waiting_req_list)) {
+        do {
                 cmd = list_first_entry(&mhba->waiting_req_list,
-                                         struct mvumi_cmd, queue_pointer);
+                                       struct mvumi_cmd, queue_pointer);
                 list_del_init(&cmd->queue_pointer);
                 result = mvumi_send_command(mhba, cmd);
                 switch (result) {
@@ -1395,65 +1925,77 @@ static void mvumi_fire_cmd(struct mvumi_hba *mhba, struct mvumi_cmd *cmd)
 
                         return;
                 }
-        }
+        } while (!list_empty(&mhba->waiting_req_list) && count--);
+
         if (num_of_cl_sent > 0)
                 mvumi_send_ib_list_entry(mhba);
 }
 
 /**
  * mvumi_enable_intr -  Enables interrupts
- * @regs:                       FW register set
+ * @mhba:               Adapter soft state
  */
-static void mvumi_enable_intr(void *regs)
+static void mvumi_enable_intr(struct mvumi_hba *mhba)
 {
         unsigned int mask;
+        struct mvumi_hw_regs *regs = mhba->regs;
 
-        iowrite32(0x3FFFFFFF, regs + CPU_ARM_TO_PCIEA_MASK_REG);
-        mask = ioread32(regs + CPU_ENPOINTA_MASK_REG);
-        mask |= INT_MAP_DL_CPU2PCIEA | INT_MAP_COMAOUT | INT_MAP_COMAERR;
-        iowrite32(mask, regs + CPU_ENPOINTA_MASK_REG);
+        iowrite32(regs->int_drbl_int_mask, regs->arm_to_pciea_mask_reg);
+        mask = ioread32(regs->enpointa_mask_reg);
+        mask |= regs->int_dl_cpu2pciea | regs->int_comaout | regs->int_comaerr;
+        iowrite32(mask, regs->enpointa_mask_reg);
 }
 
 /**
  * mvumi_disable_intr -Disables interrupt
- * @regs:                       FW register set
+ * @mhba:               Adapter soft state
  */
-static void mvumi_disable_intr(void *regs)
+static void mvumi_disable_intr(struct mvumi_hba *mhba)
 {
         unsigned int mask;
+        struct mvumi_hw_regs *regs = mhba->regs;
 
-        iowrite32(0, regs + CPU_ARM_TO_PCIEA_MASK_REG);
-        mask = ioread32(regs + CPU_ENPOINTA_MASK_REG);
-        mask &= ~(INT_MAP_DL_CPU2PCIEA | INT_MAP_COMAOUT | INT_MAP_COMAERR);
-        iowrite32(mask, regs + CPU_ENPOINTA_MASK_REG);
+        iowrite32(0, regs->arm_to_pciea_mask_reg);
+        mask = ioread32(regs->enpointa_mask_reg);
+        mask &= ~(regs->int_dl_cpu2pciea | regs->int_comaout |
+                                                        regs->int_comaerr);
+        iowrite32(mask, regs->enpointa_mask_reg);
 }
 
 static int mvumi_clear_intr(void *extend)
 {
         struct mvumi_hba *mhba = (struct mvumi_hba *) extend;
         unsigned int status, isr_status = 0, tmp = 0;
-        void *regs = mhba->mmio;
+        struct mvumi_hw_regs *regs = mhba->regs;
 
-        status = ioread32(regs + CPU_MAIN_INT_CAUSE_REG);
-        if (!(status & INT_MAP_MU) || status == 0xFFFFFFFF)
+        status = ioread32(regs->main_int_cause_reg);
+        if (!(status & regs->int_mu) || status == 0xFFFFFFFF)
                 return 1;
-        if (unlikely(status & INT_MAP_COMAERR)) {
-                tmp = ioread32(regs + CLA_ISR_CAUSE);
-                if (tmp & (CLIC_IN_ERR_IRQ | CLIC_OUT_ERR_IRQ))
-                        iowrite32(tmp & (CLIC_IN_ERR_IRQ | CLIC_OUT_ERR_IRQ),
-                                        regs + CLA_ISR_CAUSE);
-                status ^= INT_MAP_COMAERR;
+        if (unlikely(status & regs->int_comaerr)) {
+                tmp = ioread32(regs->outb_isr_cause);
+                if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580) {
+                        if (tmp & regs->clic_out_err) {
+                                iowrite32(tmp & regs->clic_out_err,
+                                                        regs->outb_isr_cause);
+                        }
+                } else {
+                        if (tmp & (regs->clic_in_err | regs->clic_out_err))
+                                iowrite32(tmp & (regs->clic_in_err |
+                                                regs->clic_out_err),
+                                                regs->outb_isr_cause);
+                }
+                status ^= mhba->regs->int_comaerr;
                 /* inbound or outbound parity error, command will timeout */
         }
-        if (status & INT_MAP_COMAOUT) {
-                tmp = ioread32(regs + CLA_ISR_CAUSE);
-                if (tmp & CLIC_OUT_IRQ)
-                        iowrite32(tmp & CLIC_OUT_IRQ, regs + CLA_ISR_CAUSE);
+        if (status & regs->int_comaout) {
+                tmp = ioread32(regs->outb_isr_cause);
+                if (tmp & regs->clic_irq)
+                        iowrite32(tmp & regs->clic_irq, regs->outb_isr_cause);
         }
-        if (status & INT_MAP_DL_CPU2PCIEA) {
-                isr_status = ioread32(regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+        if (status & regs->int_dl_cpu2pciea) {
+                isr_status = ioread32(regs->arm_to_pciea_drbl_reg);
                 if (isr_status)
-                        iowrite32(isr_status, regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+                        iowrite32(isr_status, regs->arm_to_pciea_drbl_reg);
         }
 
         mhba->global_isr = status;
@@ -1464,24 +2006,38 @@ static int mvumi_clear_intr(void *extend)
 
 /**
  * mvumi_read_fw_status_reg - returns the current FW status value
- * @regs:                       FW register set
+ * @mhba:               Adapter soft state
  */
-static unsigned int mvumi_read_fw_status_reg(void *regs)
+static unsigned int mvumi_read_fw_status_reg(struct mvumi_hba *mhba)
 {
         unsigned int status;
 
-        status = ioread32(regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+        status = ioread32(mhba->regs->arm_to_pciea_drbl_reg);
         if (status)
-                iowrite32(status, regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+                iowrite32(status, mhba->regs->arm_to_pciea_drbl_reg);
         return status;
 }
 
-static struct mvumi_instance_template mvumi_instance_template = {
+static struct mvumi_instance_template mvumi_instance_9143 = {
         .fire_cmd = mvumi_fire_cmd,
         .enable_intr = mvumi_enable_intr,
         .disable_intr = mvumi_disable_intr,
         .clear_intr = mvumi_clear_intr,
         .read_fw_status_reg = mvumi_read_fw_status_reg,
+        .check_ib_list = mvumi_check_ib_list_9143,
+        .check_ob_list = mvumi_check_ob_list_9143,
+        .reset_host = mvumi_reset_host_9143,
+};
+
+static struct mvumi_instance_template mvumi_instance_9580 = {
+        .fire_cmd = mvumi_fire_cmd,
+        .enable_intr = mvumi_enable_intr,
+        .disable_intr = mvumi_disable_intr,
+        .clear_intr = mvumi_clear_intr,
+        .read_fw_status_reg = mvumi_read_fw_status_reg,
+        .check_ib_list = mvumi_check_ib_list_9580,
+        .check_ob_list = mvumi_check_ob_list_9580,
+        .reset_host = mvumi_reset_host_9580,
 };
 
 static int mvumi_slave_configure(struct scsi_device *sdev)
@@ -1681,6 +2237,124 @@ static struct scsi_transport_template mvumi_transport_template = {
         .eh_timed_out = mvumi_timed_out,
 };
 
+static int mvumi_cfg_hw_reg(struct mvumi_hba *mhba)
+{
+        void *base = NULL;
+        struct mvumi_hw_regs *regs;
+
+        switch (mhba->pdev->device) {
+        case PCI_DEVICE_ID_MARVELL_MV9143:
+                mhba->mmio = mhba->base_addr[0];
+                base = mhba->mmio;
+                if (!mhba->regs) {
+                        mhba->regs = kzalloc(sizeof(*regs), GFP_KERNEL);
+                        if (mhba->regs == NULL)
+                                return -ENOMEM;
+                }
+                regs = mhba->regs;
+
+                /* For Arm */
+                regs->ctrl_sts_reg          = base + 0x20104;
+                regs->rstoutn_mask_reg      = base + 0x20108;
+                regs->sys_soft_rst_reg      = base + 0x2010C;
+                regs->main_int_cause_reg    = base + 0x20200;
+                regs->enpointa_mask_reg     = base + 0x2020C;
+                regs->rstoutn_en_reg        = base + 0xF1400;
+                /* For Doorbell */
+                regs->pciea_to_arm_drbl_reg = base + 0x20400;
+                regs->arm_to_pciea_drbl_reg = base + 0x20408;
+                regs->arm_to_pciea_mask_reg = base + 0x2040C;
+                regs->pciea_to_arm_msg0     = base + 0x20430;
+                regs->pciea_to_arm_msg1     = base + 0x20434;
+                regs->arm_to_pciea_msg0     = base + 0x20438;
+                regs->arm_to_pciea_msg1     = base + 0x2043C;
+
+                /* For Message Unit */
+
+                regs->inb_aval_count_basel  = base + 0x508;
+                regs->inb_aval_count_baseh  = base + 0x50C;
+                regs->inb_write_pointer     = base + 0x518;
+                regs->inb_read_pointer      = base + 0x51C;
+                regs->outb_coal_cfg         = base + 0x568;
+                regs->outb_copy_basel       = base + 0x5B0;
+                regs->outb_copy_baseh       = base + 0x5B4;
+                regs->outb_copy_pointer     = base + 0x544;
+                regs->outb_read_pointer     = base + 0x548;
+                regs->outb_isr_cause        = base + 0x560;
+                regs->outb_coal_cfg         = base + 0x568;
+                /* Bit setting for HW */
+                regs->int_comaout           = 1 << 8;
+                regs->int_comaerr           = 1 << 6;
+                regs->int_dl_cpu2pciea      = 1 << 1;
+                regs->cl_pointer_toggle     = 1 << 12;
+                regs->clic_irq              = 1 << 1;
+                regs->clic_in_err           = 1 << 8;
+                regs->clic_out_err          = 1 << 12;
+                regs->cl_slot_num_mask      = 0xFFF;
+                regs->int_drbl_int_mask     = 0x3FFFFFFF;
+                regs->int_mu = regs->int_dl_cpu2pciea | regs->int_comaout |
+                                                        regs->int_comaerr;
+                break;
+        case PCI_DEVICE_ID_MARVELL_MV9580:
+                mhba->mmio = mhba->base_addr[2];
+                base = mhba->mmio;
+                if (!mhba->regs) {
+                        mhba->regs = kzalloc(sizeof(*regs), GFP_KERNEL);
+                        if (mhba->regs == NULL)
+                                return -ENOMEM;
+                }
+                regs = mhba->regs;
+                /* For Arm */
+                regs->ctrl_sts_reg          = base + 0x20104;
+                regs->rstoutn_mask_reg      = base + 0x1010C;
+                regs->sys_soft_rst_reg      = base + 0x10108;
+                regs->main_int_cause_reg    = base + 0x10200;
+                regs->enpointa_mask_reg     = base + 0x1020C;
+                regs->rstoutn_en_reg        = base + 0xF1400;
+
+                /* For Doorbell */
+                regs->pciea_to_arm_drbl_reg = base + 0x10460;
+                regs->arm_to_pciea_drbl_reg = base + 0x10480;
+                regs->arm_to_pciea_mask_reg = base + 0x10484;
+                regs->pciea_to_arm_msg0     = base + 0x10400;
+                regs->pciea_to_arm_msg1     = base + 0x10404;
+                regs->arm_to_pciea_msg0     = base + 0x10420;
+                regs->arm_to_pciea_msg1     = base + 0x10424;
+
+                /* For reset*/
+                regs->reset_request         = base + 0x10108;
+                regs->reset_enable          = base + 0x1010c;
+
+                /* For Message Unit */
+                regs->inb_aval_count_basel  = base + 0x4008;
+                regs->inb_aval_count_baseh  = base + 0x400C;
+                regs->inb_write_pointer     = base + 0x4018;
+                regs->inb_read_pointer      = base + 0x401C;
+                regs->outb_copy_basel       = base + 0x4058;
+                regs->outb_copy_baseh       = base + 0x405C;
+                regs->outb_copy_pointer     = base + 0x406C;
+                regs->outb_read_pointer     = base + 0x4070;
+                regs->outb_coal_cfg         = base + 0x4080;
+                regs->outb_isr_cause        = base + 0x4088;
+                /* Bit setting for HW */
+                regs->int_comaout           = 1 << 4;
+                regs->int_dl_cpu2pciea      = 1 << 12;
+                regs->int_comaerr           = 1 << 29;
+                regs->cl_pointer_toggle     = 1 << 14;
+                regs->cl_slot_num_mask      = 0x3FFF;
+                regs->clic_irq              = 1 << 0;
+                regs->clic_out_err          = 1 << 1;
+                regs->int_drbl_int_mask     = 0x3FFFFFFF;
+                regs->int_mu = regs->int_dl_cpu2pciea | regs->int_comaout;
+                break;
+        default:
+                return -1;
+                break;
+        }
+
+        return 0;
+}
+
 /**
  * mvumi_init_fw -      Initializes the FW
  * @mhba:               Adapter soft state
@@ -1699,15 +2373,18 @@ static int mvumi_init_fw(struct mvumi_hba *mhba)
         if (ret)
                 goto fail_ioremap;
 
-        mhba->mmio = mhba->base_addr[0];
-
         switch (mhba->pdev->device) {
         case PCI_DEVICE_ID_MARVELL_MV9143:
-                mhba->instancet = &mvumi_instance_template;
+                mhba->instancet = &mvumi_instance_9143;
                 mhba->io_seq = 0;
                 mhba->max_sge = MVUMI_MAX_SG_ENTRY;
                 mhba->request_id_enabled = 1;
                 break;
+        case PCI_DEVICE_ID_MARVELL_MV9580:
+                mhba->instancet = &mvumi_instance_9580;
+                mhba->io_seq = 0;
+                mhba->max_sge = MVUMI_MAX_SG_ENTRY;
+                break;
         default:
                 dev_err(&mhba->pdev->dev, "device 0x%x not supported!\n",
                                                         mhba->pdev->device);
@@ -1717,15 +2394,21 @@ static int mvumi_init_fw(struct mvumi_hba *mhba)
         }
         dev_dbg(&mhba->pdev->dev, "device id : %04X is found.\n",
                                                         mhba->pdev->device);
-
-        mhba->handshake_page = kzalloc(HSP_MAX_SIZE, GFP_KERNEL);
+        ret = mvumi_cfg_hw_reg(mhba);
+        if (ret) {
+                dev_err(&mhba->pdev->dev,
+                        "failed to allocate memory for reg\n");
+                ret = -ENOMEM;
+                goto fail_alloc_mem;
+        }
+        mhba->handshake_page = pci_alloc_consistent(mhba->pdev, HSP_MAX_SIZE,
+                                                &mhba->handshake_page_phys);
         if (!mhba->handshake_page) {
                 dev_err(&mhba->pdev->dev,
                         "failed to allocate memory for handshake\n");
                 ret = -ENOMEM;
-                goto fail_alloc_mem;
+                goto fail_alloc_page;
         }
-        mhba->handshake_page_phys = virt_to_phys(mhba->handshake_page);
 
         if (mvumi_start(mhba)) {
                 ret = -EINVAL;
@@ -1739,7 +2422,10 @@ static int mvumi_init_fw(struct mvumi_hba *mhba)
 
 fail_ready_state:
         mvumi_release_mem_resource(mhba);
-        kfree(mhba->handshake_page);
+        pci_free_consistent(mhba->pdev, HSP_MAX_SIZE,
+                mhba->handshake_page, mhba->handshake_page_phys);
+fail_alloc_page:
+        kfree(mhba->regs);
 fail_alloc_mem:
         mvumi_unmap_pci_addr(mhba->pdev, mhba->base_addr);
 fail_ioremap:
@@ -1755,6 +2441,7 @@ fail_ioremap:
 static int mvumi_io_attach(struct mvumi_hba *mhba)
 {
         struct Scsi_Host *host = mhba->shost;
+        struct scsi_device *sdev = NULL;
         int ret;
         unsigned int max_sg = (mhba->ib_max_size + 4 -
                 sizeof(struct mvumi_msg_frame)) / sizeof(struct mvumi_sgl);
@@ -1764,7 +2451,7 @@ static int mvumi_io_attach(struct mvumi_hba *mhba)
         host->can_queue = (mhba->max_io - 1) ? (mhba->max_io - 1) : 1;
         host->sg_tablesize = mhba->max_sge > max_sg ? max_sg : mhba->max_sge;
         host->max_sectors = mhba->max_transfer_size / 512;
-        host->cmd_per_lun =  (mhba->max_io - 1) ? (mhba->max_io - 1) : 1;
+        host->cmd_per_lun = (mhba->max_io - 1) ? (mhba->max_io - 1) : 1;
         host->max_id = mhba->max_target_id;
         host->max_cmd_len = MAX_COMMAND_SIZE;
         host->transportt = &mvumi_transport_template;
@@ -1775,9 +2462,43 @@ static int mvumi_io_attach(struct mvumi_hba *mhba)
                 return ret;
         }
         mhba->fw_flag |= MVUMI_FW_ATTACH;
-        scsi_scan_host(host);
 
+        mutex_lock(&mhba->sas_discovery_mutex);
+        if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580)
+                ret = scsi_add_device(host, 0, mhba->max_target_id - 1, 0);
+        else
+                ret = 0;
+        if (ret) {
+                dev_err(&mhba->pdev->dev, "add virtual device failed\n");
+                mutex_unlock(&mhba->sas_discovery_mutex);
+                goto fail_add_device;
+        }
+
+        mhba->dm_thread = kthread_create(mvumi_rescan_bus,
+                                                mhba, "mvumi_scanthread");
+        if (IS_ERR(mhba->dm_thread)) {
+                dev_err(&mhba->pdev->dev,
+                        "failed to create device scan thread\n");
+                mutex_unlock(&mhba->sas_discovery_mutex);
+                goto fail_create_thread;
+        }
+        atomic_set(&mhba->pnp_count, 1);
+        wake_up_process(mhba->dm_thread);
+
+        mutex_unlock(&mhba->sas_discovery_mutex);
         return 0;
+
+fail_create_thread:
+        if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580)
+                sdev = scsi_device_lookup(mhba->shost, 0,
+                                                mhba->max_target_id - 1, 0);
+        if (sdev) {
+                scsi_remove_device(sdev);
+                scsi_device_put(sdev);
+        }
+fail_add_device:
+        scsi_remove_host(mhba->shost);
+        return ret;
 }
 
 /**
@@ -1828,8 +2549,12 @@ static int __devinit mvumi_probe_one(struct pci_dev *pdev,
         INIT_LIST_HEAD(&mhba->free_ob_list);
         INIT_LIST_HEAD(&mhba->res_list);
         INIT_LIST_HEAD(&mhba->waiting_req_list);
+        mutex_init(&mhba->device_lock);
+        INIT_LIST_HEAD(&mhba->mhba_dev_list);
+        INIT_LIST_HEAD(&mhba->shost_dev_list);
         atomic_set(&mhba->fw_outstanding, 0);
         init_waitqueue_head(&mhba->int_cmd_wait_q);
+        mutex_init(&mhba->sas_discovery_mutex);
 
         mhba->pdev = pdev;
         mhba->shost = host;
@@ -1845,19 +2570,22 @@ static int __devinit mvumi_probe_one(struct pci_dev *pdev,
                 dev_err(&pdev->dev, "failed to register IRQ\n");
                 goto fail_init_irq;
         }
-        mhba->instancet->enable_intr(mhba->mmio);
+
+        mhba->instancet->enable_intr(mhba);
         pci_set_drvdata(pdev, mhba);
 
         ret = mvumi_io_attach(mhba);
         if (ret)
                 goto fail_io_attach;
+
+        mvumi_backup_bar_addr(mhba);
         dev_dbg(&pdev->dev, "probe mvumi driver successfully.\n");
 
         return 0;
 
 fail_io_attach:
         pci_set_drvdata(pdev, NULL);
-        mhba->instancet->disable_intr(mhba->mmio);
+        mhba->instancet->disable_intr(mhba);
         free_irq(mhba->pdev->irq, mhba);
 fail_init_irq:
         mvumi_release_fw(mhba);
@@ -1877,11 +2605,17 @@ static void mvumi_detach_one(struct pci_dev *pdev)
         struct mvumi_hba *mhba;
 
         mhba = pci_get_drvdata(pdev);
+        if (mhba->dm_thread) {
+                kthread_stop(mhba->dm_thread);
+                mhba->dm_thread = NULL;
+        }
+
+        mvumi_detach_devices(mhba);
         host = mhba->shost;
         scsi_remove_host(mhba->shost);
         mvumi_flush_cache(mhba);
 
-        mhba->instancet->disable_intr(mhba->mmio);
+        mhba->instancet->disable_intr(mhba);
         free_irq(mhba->pdev->irq, mhba);
         mvumi_release_fw(mhba);
         scsi_host_put(host);
@@ -1909,7 +2643,7 @@ static int mvumi_suspend(struct pci_dev *pdev, pm_message_t state)
         mvumi_flush_cache(mhba);
 
         pci_set_drvdata(pdev, mhba);
-        mhba->instancet->disable_intr(mhba->mmio);
+        mhba->instancet->disable_intr(mhba);
         free_irq(mhba->pdev->irq, mhba);
         mvumi_unmap_pci_addr(pdev, mhba->base_addr);
         pci_release_regions(pdev);
@@ -1956,8 +2690,13 @@ static int mvumi_resume(struct pci_dev *pdev)
         if (ret)
                 goto release_regions;
 
+        if (mvumi_cfg_hw_reg(mhba)) {
+                ret = -EINVAL;
+                goto unmap_pci_addr;
+        }
+
         mhba->mmio = mhba->base_addr[0];
-        mvumi_reset(mhba->mmio);
+        mvumi_reset(mhba);
 
         if (mvumi_start(mhba)) {
                 ret = -EINVAL;
@@ -1970,7 +2709,7 @@ static int mvumi_resume(struct pci_dev *pdev)
                 dev_err(&pdev->dev, "failed to register IRQ\n");
                 goto unmap_pci_addr;
         }
-        mhba->instancet->enable_intr(mhba->mmio);
+        mhba->instancet->enable_intr(mhba);
 
         return 0;
 
diff --git a/drivers/scsi/mvumi.h b/drivers/scsi/mvumi.h
index 10b9237566f0..e360135fd1bd 100644
--- a/drivers/scsi/mvumi.h
+++ b/drivers/scsi/mvumi.h
@@ -34,51 +34,87 @@
 #define MV_DRIVER_NAME                  "mvumi"
 #define PCI_VENDOR_ID_MARVELL_2         0x1b4b
 #define PCI_DEVICE_ID_MARVELL_MV9143    0x9143
+#define PCI_DEVICE_ID_MARVELL_MV9580    0x9580
 
 #define MVUMI_INTERNAL_CMD_WAIT_TIME    45
+#define MVUMI_INQUIRY_LENGTH            44
+#define MVUMI_INQUIRY_UUID_OFF          36
+#define MVUMI_INQUIRY_UUID_LEN          8
 
 #define IS_DMA64                        (sizeof(dma_addr_t) == 8)
 
 enum mvumi_qc_result {
-        MV_QUEUE_COMMAND_RESULT_SENT    = 0,
+        MV_QUEUE_COMMAND_RESULT_SENT = 0,
         MV_QUEUE_COMMAND_RESULT_NO_RESOURCE,
 };
 
-enum {
-        /*******************************************/
-
-        /* ARM Mbus Registers Map       */
-
-        /*******************************************/
-        CPU_MAIN_INT_CAUSE_REG  = 0x20200,
-        CPU_MAIN_IRQ_MASK_REG   = 0x20204,
-        CPU_MAIN_FIQ_MASK_REG   = 0x20208,
-        CPU_ENPOINTA_MASK_REG   = 0x2020C,
-        CPU_ENPOINTB_MASK_REG   = 0x20210,
-
-        INT_MAP_COMAERR         = 1 << 6,
-        INT_MAP_COMAIN          = 1 << 7,
-        INT_MAP_COMAOUT         = 1 << 8,
-        INT_MAP_COMBERR         = 1 << 9,
-        INT_MAP_COMBIN          = 1 << 10,
-        INT_MAP_COMBOUT         = 1 << 11,
-
-        INT_MAP_COMAINT = (INT_MAP_COMAOUT | INT_MAP_COMAERR),
-        INT_MAP_COMBINT = (INT_MAP_COMBOUT | INT_MAP_COMBIN | INT_MAP_COMBERR),
-
-        INT_MAP_DL_PCIEA2CPU    = 1 << 0,
-        INT_MAP_DL_CPU2PCIEA    = 1 << 1,
-
-        /***************************************/
+struct mvumi_hw_regs {
+        /* For CPU */
+        void *main_int_cause_reg;
+        void *enpointa_mask_reg;
+        void *enpointb_mask_reg;
+        void *rstoutn_en_reg;
+        void *ctrl_sts_reg;
+        void *rstoutn_mask_reg;
+        void *sys_soft_rst_reg;
+
+        /* For Doorbell */
+        void *pciea_to_arm_drbl_reg;
+        void *arm_to_pciea_drbl_reg;
+        void *arm_to_pciea_mask_reg;
+        void *pciea_to_arm_msg0;
+        void *pciea_to_arm_msg1;
+        void *arm_to_pciea_msg0;
+        void *arm_to_pciea_msg1;
+
+        /* reset register */
+        void *reset_request;
+        void *reset_enable;
+
+        /* For Message Unit */
+        void *inb_list_basel;
+        void *inb_list_baseh;
+        void *inb_aval_count_basel;
+        void *inb_aval_count_baseh;
+        void *inb_write_pointer;
+        void *inb_read_pointer;
+        void *outb_list_basel;
+        void *outb_list_baseh;
+        void *outb_copy_basel;
+        void *outb_copy_baseh;
+        void *outb_copy_pointer;
+        void *outb_read_pointer;
+        void *inb_isr_cause;
+        void *outb_isr_cause;
+        void *outb_coal_cfg;
+        void *outb_coal_timeout;
+
+        /* Bit setting for HW */
+        u32 int_comaout;
+        u32 int_comaerr;
+        u32 int_dl_cpu2pciea;
+        u32 int_mu;
+        u32 int_drbl_int_mask;
+        u32 int_main_int_mask;
+        u32 cl_pointer_toggle;
+        u32 cl_slot_num_mask;
+        u32 clic_irq;
+        u32 clic_in_err;
+        u32 clic_out_err;
+};
 
-        /* ARM Doorbell Registers Map           */
+struct mvumi_dyn_list_entry {
+        u32 src_low_addr;
+        u32 src_high_addr;
+        u32 if_length;
+        u32 reserve;
+};
 
-        /***************************************/
-        CPU_PCIEA_TO_ARM_DRBL_REG       = 0x20400,
-        CPU_PCIEA_TO_ARM_MASK_REG       = 0x20404,
-        CPU_ARM_TO_PCIEA_DRBL_REG       = 0x20408,
-        CPU_ARM_TO_PCIEA_MASK_REG       = 0x2040C,
+#define SCSI_CMD_MARVELL_SPECIFIC       0xE1
+#define CDB_CORE_MODULE                 0x1
+#define CDB_CORE_SHUTDOWN               0xB
 
+enum {
         DRBL_HANDSHAKE                  = 1 << 0,
         DRBL_SOFT_RESET                 = 1 << 1,
         DRBL_BUS_CHANGE                 = 1 << 2,
@@ -86,46 +122,6 @@ enum {
         DRBL_MU_RESET                   = 1 << 4,
         DRBL_HANDSHAKE_ISR              = DRBL_HANDSHAKE,
 
-        CPU_PCIEA_TO_ARM_MSG0           = 0x20430,
-        CPU_PCIEA_TO_ARM_MSG1           = 0x20434,
-        CPU_ARM_TO_PCIEA_MSG0           = 0x20438,
-        CPU_ARM_TO_PCIEA_MSG1           = 0x2043C,
-
-        /*******************************************/
-
-        /* ARM Communication List Registers Map    */
-
-        /*******************************************/
-        CLA_INB_LIST_BASEL              = 0x500,
-        CLA_INB_LIST_BASEH              = 0x504,
-        CLA_INB_AVAL_COUNT_BASEL        = 0x508,
-        CLA_INB_AVAL_COUNT_BASEH        = 0x50C,
-        CLA_INB_DESTI_LIST_BASEL        = 0x510,
-        CLA_INB_DESTI_LIST_BASEH        = 0x514,
-        CLA_INB_WRITE_POINTER           = 0x518,
-        CLA_INB_READ_POINTER            = 0x51C,
-
-        CLA_OUTB_LIST_BASEL             = 0x530,
-        CLA_OUTB_LIST_BASEH             = 0x534,
-        CLA_OUTB_SOURCE_LIST_BASEL      = 0x538,
-        CLA_OUTB_SOURCE_LIST_BASEH      = 0x53C,
-        CLA_OUTB_COPY_POINTER           = 0x544,
-        CLA_OUTB_READ_POINTER           = 0x548,
-
-        CLA_ISR_CAUSE                   = 0x560,
-        CLA_ISR_MASK                    = 0x564,
-
-        INT_MAP_MU              = (INT_MAP_DL_CPU2PCIEA | INT_MAP_COMAINT),
-
-        CL_POINTER_TOGGLE               = 1 << 12,
-
-        CLIC_IN_IRQ                     = 1 << 0,
-        CLIC_OUT_IRQ                    = 1 << 1,
-        CLIC_IN_ERR_IRQ                 = 1 << 8,
-        CLIC_OUT_ERR_IRQ                = 1 << 12,
-
-        CL_SLOT_NUM_MASK                = 0xFFF,
-
         /*
         * Command flag is the flag for the CDB command itself
         */
@@ -137,15 +133,23 @@ enum {
         CMD_FLAG_DATA_IN                = 1 << 3,
         /* 1-host write data */
         CMD_FLAG_DATA_OUT               = 1 << 4,
-
-        SCSI_CMD_MARVELL_SPECIFIC       = 0xE1,
-        CDB_CORE_SHUTDOWN               = 0xB,
+        CMD_FLAG_PRDT_IN_HOST           = 1 << 5,
 };
 
 #define APICDB0_EVENT                   0xF4
 #define APICDB1_EVENT_GETEVENT          0
+#define APICDB1_HOST_GETEVENT           1
 #define MAX_EVENTS_RETURNED             6
 
+#define DEVICE_OFFLINE  0
+#define DEVICE_ONLINE   1
+
+struct mvumi_hotplug_event {
+        u16 size;
+        u8 dummy[2];
+        u8 bitmap[0];
+};
+
 struct mvumi_driver_event {
         u32     time_stamp;
         u32     sequence_no;
@@ -172,8 +176,14 @@ struct mvumi_events_wq {
         void *param;
 };
 
+#define HS_CAPABILITY_SUPPORT_COMPACT_SG        (1U << 4)
+#define HS_CAPABILITY_SUPPORT_PRD_HOST          (1U << 5)
+#define HS_CAPABILITY_SUPPORT_DYN_SRC           (1U << 6)
+#define HS_CAPABILITY_NEW_PAGE_IO_DEPTH_DEF     (1U << 14)
+
 #define MVUMI_MAX_SG_ENTRY      32
 #define SGD_EOT                 (1L << 27)
+#define SGD_EOT_CP              (1L << 22)
 
 struct mvumi_sgl {
         u32     baseaddr_l;
@@ -181,6 +191,39 @@ struct mvumi_sgl {
         u32     flags;
         u32     size;
 };
+struct mvumi_compact_sgl {
+        u32     baseaddr_l;
+        u32     baseaddr_h;
+        u32     flags;
+};
+
+#define GET_COMPACT_SGD_SIZE(sgd)       \
+        ((((struct mvumi_compact_sgl *)(sgd))->flags) & 0x3FFFFFL)
+
+#define SET_COMPACT_SGD_SIZE(sgd, sz) do {                      \
+        (((struct mvumi_compact_sgl *)(sgd))->flags) &= ~0x3FFFFFL;     \
+        (((struct mvumi_compact_sgl *)(sgd))->flags) |= (sz);           \
+} while (0)
+#define sgd_getsz(_mhba, sgd, sz) do {                          \
+        if (_mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)   \
+                (sz) = GET_COMPACT_SGD_SIZE(sgd);       \
+        else \
+                (sz) = (sgd)->size;                     \
+} while (0)
+
+#define sgd_setsz(_mhba, sgd, sz) do {                          \
+        if (_mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)   \
+                SET_COMPACT_SGD_SIZE(sgd, sz);          \
+        else \
+                (sgd)->size = (sz);                     \
+} while (0)
+
+#define sgd_inc(_mhba, sgd) do {        \
+        if (_mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)   \
+                sgd = (struct mvumi_sgl *)(((unsigned char *) (sgd)) + 12); \
+        else \
+                sgd = (struct mvumi_sgl *)(((unsigned char *) (sgd)) + 16); \
+} while (0)
 
 struct mvumi_res {
         struct list_head entry;
@@ -197,7 +240,7 @@ enum resource_type {
 };
 
 struct mvumi_sense_data {
-        u8 error_eode:7;
+        u8 error_code:7;
         u8 valid:1;
         u8 segment_number;
         u8 sense_key:4;
@@ -220,6 +263,7 @@ struct mvumi_sense_data {
 struct mvumi_cmd {
         struct list_head queue_pointer;
         struct mvumi_msg_frame *frame;
+        dma_addr_t frame_phys;
         struct scsi_cmnd *scmd;
         atomic_t sync_cmd;
         void *data_buf;
@@ -393,7 +437,8 @@ struct mvumi_hs_page2 {
         u16 frame_length;
 
         u8 host_type;
-        u8 reserved[3];
+        u8 host_cap;
+        u8 reserved[2];
         struct version_info host_ver;
         u32 system_io_bus;
         u32 slot_number;
@@ -435,8 +480,17 @@ struct mvumi_tag {
         unsigned short size;
 };
 
+struct mvumi_device {
+        struct list_head list;
+        struct scsi_device *sdev;
+        u64     wwid;
+        u8      dev_type;
+        int     id;
+};
+
 struct mvumi_hba {
         void *base_addr[MAX_BASE_ADDRESS];
+        u32 pci_base[MAX_BASE_ADDRESS];
         void *mmio;
         struct list_head cmd_pool;
         struct Scsi_Host *shost;
@@ -449,6 +503,9 @@ struct mvumi_hba {
         void *ib_list;
         dma_addr_t ib_list_phys;
 
+        void *ib_frame;
+        dma_addr_t ib_frame_phys;
+
         void *ob_list;
         dma_addr_t ob_list_phys;
 
@@ -477,12 +534,14 @@ struct mvumi_hba {
         unsigned char hba_total_pages;
         unsigned char fw_flag;
         unsigned char request_id_enabled;
+        unsigned char eot_flag;
         unsigned short hba_capability;
         unsigned short io_seq;
 
         unsigned int ib_cur_slot;
         unsigned int ob_cur_slot;
         unsigned int fw_state;
+        struct mutex sas_discovery_mutex;
 
         struct list_head ob_data_list;
         struct list_head free_ob_list;
@@ -491,14 +550,24 @@ struct mvumi_hba {
 
         struct mvumi_tag tag_pool;
         struct mvumi_cmd **tag_cmd;
+        struct mvumi_hw_regs *regs;
+        struct mutex device_lock;
+        struct list_head mhba_dev_list;
+        struct list_head shost_dev_list;
+        struct task_struct *dm_thread;
+        atomic_t pnp_count;
 };
 
 struct mvumi_instance_template {
-        void (*fire_cmd)(struct mvumi_hba *, struct mvumi_cmd *);
-        void (*enable_intr)(void *) ;
-        void (*disable_intr)(void *);
-        int (*clear_intr)(void *);
-        unsigned int (*read_fw_status_reg)(void *);
+        void (*fire_cmd) (struct mvumi_hba *, struct mvumi_cmd *);
+        void (*enable_intr) (struct mvumi_hba *);
+        void (*disable_intr) (struct mvumi_hba *);
+        int (*clear_intr) (void *);
+        unsigned int (*read_fw_status_reg) (struct mvumi_hba *);
+        unsigned int (*check_ib_list) (struct mvumi_hba *);
+        int (*check_ob_list) (struct mvumi_hba *, unsigned int *,
+                              unsigned int *);
+        int (*reset_host) (struct mvumi_hba *);
 };
 
 extern struct timezone sys_tz;
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 799a58bb9859..48fca47384b7 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -2080,6 +2080,7 @@ qla2x00_configure_hba(scsi_qla_host_t *vha)
         uint8_t       domain;
         char            connect_type[22];
         struct qla_hw_data *ha = vha->hw;
+        unsigned long flags;
 
         /* Get host addresses. */
         rval = qla2x00_get_adapter_id(vha,
@@ -2154,9 +2155,9 @@ qla2x00_configure_hba(scsi_qla_host_t *vha)
         vha->d_id.b.area = area;
         vha->d_id.b.al_pa = al_pa;
 
-        spin_lock(&ha->vport_slock);
+        spin_lock_irqsave(&ha->vport_slock, flags);
         qlt_update_vp_map(vha, SET_AL_PA);
-        spin_unlock(&ha->vport_slock);
+        spin_unlock_irqrestore(&ha->vport_slock, flags);
 
         if (!vha->flags.init_done)
                 ql_log(ql_log_info, vha, 0x2010,
diff --git a/drivers/scsi/qla2xxx/qla_mid.c b/drivers/scsi/qla2xxx/qla_mid.c
index bd4708a422cd..20fd974f903a 100644
--- a/drivers/scsi/qla2xxx/qla_mid.c
+++ b/drivers/scsi/qla2xxx/qla_mid.c
@@ -149,6 +149,7 @@ qla2x00_mark_vp_devices_dead(scsi_qla_host_t *vha)
 int
 qla24xx_disable_vp(scsi_qla_host_t *vha)
 {
+        unsigned long flags;
         int ret;
 
         ret = qla24xx_control_vp(vha, VCE_COMMAND_DISABLE_VPS_LOGO_ALL);
@@ -156,7 +157,9 @@ qla24xx_disable_vp(scsi_qla_host_t *vha)
         atomic_set(&vha->loop_down_timer, LOOP_DOWN_TIME);
 
         /* Remove port id from vp target map */
+        spin_lock_irqsave(&vha->hw->vport_slock, flags);
         qlt_update_vp_map(vha, RESET_AL_PA);
+        spin_unlock_irqrestore(&vha->hw->vport_slock, flags);
 
         qla2x00_mark_vp_devices_dead(vha);
         atomic_set(&vha->vp_state, VP_FAILED);
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index bddc97c5c8e9..62aa5584f644 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -557,6 +557,7 @@ static bool qlt_check_fcport_exist(struct scsi_qla_host *vha,
         int pmap_len;
         fc_port_t *fcport;
         int global_resets;
+        unsigned long flags;
 
 retry:
         global_resets = atomic_read(&ha->tgt.qla_tgt->tgt_global_resets_count);
@@ -625,10 +626,10 @@ retry:
             sess->s_id.b.area, sess->loop_id, fcport->d_id.b.domain,
             fcport->d_id.b.al_pa, fcport->d_id.b.area, fcport->loop_id);
 
-        sess->s_id = fcport->d_id;
-        sess->loop_id = fcport->loop_id;
-        sess->conf_compl_supported = !!(fcport->flags &
-            FCF_CONF_COMP_SUPPORTED);
+        spin_lock_irqsave(&ha->hardware_lock, flags);
+        ha->tgt.tgt_ops->update_sess(sess, fcport->d_id, fcport->loop_id,
+                                (fcport->flags & FCF_CONF_COMP_SUPPORTED));
+        spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
         res = true;
 
@@ -740,10 +741,9 @@ static struct qla_tgt_sess *qlt_create_sess(
                                 qlt_undelete_sess(sess);
 
                         kref_get(&sess->se_sess->sess_kref);
-                        sess->s_id = fcport->d_id;
-                        sess->loop_id = fcport->loop_id;
-                        sess->conf_compl_supported = !!(fcport->flags &
-                            FCF_CONF_COMP_SUPPORTED);
+                        ha->tgt.tgt_ops->update_sess(sess, fcport->d_id, fcport->loop_id,
+                                                (fcport->flags & FCF_CONF_COMP_SUPPORTED));
+
                         if (sess->local && !local)
                                 sess->local = 0;
                         spin_unlock_irqrestore(&ha->hardware_lock, flags);
@@ -796,8 +796,7 @@ static struct qla_tgt_sess *qlt_create_sess(
          */
         kref_get(&sess->se_sess->sess_kref);
 
-        sess->conf_compl_supported = !!(fcport->flags &
-            FCF_CONF_COMP_SUPPORTED);
+        sess->conf_compl_supported = (fcport->flags & FCF_CONF_COMP_SUPPORTED);
         BUILD_BUG_ON(sizeof(sess->port_name) != sizeof(fcport->port_name));
         memcpy(sess->port_name, fcport->port_name, sizeof(sess->port_name));
 
@@ -869,10 +868,8 @@ void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
                         ql_dbg(ql_dbg_tgt_mgt, vha, 0xf007,
                             "Reappeared sess %p\n", sess);
                 }
-                sess->s_id = fcport->d_id;
-                sess->loop_id = fcport->loop_id;
-                sess->conf_compl_supported = !!(fcport->flags &
-                    FCF_CONF_COMP_SUPPORTED);
+                ha->tgt.tgt_ops->update_sess(sess, fcport->d_id, fcport->loop_id,
+                                        (fcport->flags & FCF_CONF_COMP_SUPPORTED));
         }
 
         if (sess && sess->local) {
@@ -1403,7 +1400,7 @@ static void qlt_24xx_send_task_mgmt_ctio(struct scsi_qla_host *ha,
         ctio->u.status1.scsi_status =
             __constant_cpu_to_le16(SS_RESPONSE_INFO_LEN_VALID);
         ctio->u.status1.response_len = __constant_cpu_to_le16(8);
-        ((uint32_t *)ctio->u.status1.sense_data)[0] = cpu_to_be32(resp_code);
+        ctio->u.status1.sense_data[0] = resp_code;
 
         qla2x00_start_iocbs(ha, ha->req);
 }
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index 170af1571214..bad749561ec2 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -648,6 +648,7 @@ struct qla_tgt_func_tmpl {
 
         int (*check_initiator_node_acl)(struct scsi_qla_host *, unsigned char *,
                                         void *, uint8_t *, uint16_t);
+        void (*update_sess)(struct qla_tgt_sess *, port_id_t, uint16_t, bool);
         struct qla_tgt_sess *(*find_sess_by_loop_id)(struct scsi_qla_host *,
                                                 const uint16_t);
         struct qla_tgt_sess *(*find_sess_by_s_id)(struct scsi_qla_host *,
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 4752f65a9272..3d74f2f39ae1 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -237,7 +237,7 @@ static char *tcm_qla2xxx_get_fabric_wwn(struct se_portal_group *se_tpg)
                                 struct tcm_qla2xxx_tpg, se_tpg);
         struct tcm_qla2xxx_lport *lport = tpg->lport;
 
-        return &lport->lport_name[0];
+        return lport->lport_naa_name;
 }
 
 static char *tcm_qla2xxx_npiv_get_fabric_wwn(struct se_portal_group *se_tpg)
@@ -735,17 +735,6 @@ static int tcm_qla2xxx_queue_tm_rsp(struct se_cmd *se_cmd)
         return 0;
 }
 
-static u16 tcm_qla2xxx_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
-static u16 tcm_qla2xxx_set_fabric_sense_len(struct se_cmd *se_cmd,
-                                        u32 sense_length)
-{
-        return 0;
-}
-
 /* Local pointer to allocated TCM configfs fabric module */
 struct target_fabric_configfs *tcm_qla2xxx_fabric_configfs;
 struct target_fabric_configfs *tcm_qla2xxx_npiv_fabric_configfs;
@@ -1468,6 +1457,78 @@ static int tcm_qla2xxx_check_initiator_node_acl(
         return 0;
 }
 
+static void tcm_qla2xxx_update_sess(struct qla_tgt_sess *sess, port_id_t s_id,
+                                    uint16_t loop_id, bool conf_compl_supported)
+{
+        struct qla_tgt *tgt = sess->tgt;
+        struct qla_hw_data *ha = tgt->ha;
+        struct tcm_qla2xxx_lport *lport = ha->tgt.target_lport_ptr;
+        struct se_node_acl *se_nacl = sess->se_sess->se_node_acl;
+        struct tcm_qla2xxx_nacl *nacl = container_of(se_nacl,
+                        struct tcm_qla2xxx_nacl, se_node_acl);
+        u32 key;
+
+
+        if (sess->loop_id != loop_id || sess->s_id.b24 != s_id.b24)
+                pr_info("Updating session %p from port %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x loop_id %d -> %d s_id %x:%x:%x -> %x:%x:%x\n",
+                        sess,
+                        sess->port_name[0], sess->port_name[1],
+                        sess->port_name[2], sess->port_name[3],
+                        sess->port_name[4], sess->port_name[5],
+                        sess->port_name[6], sess->port_name[7],
+                        sess->loop_id, loop_id,
+                        sess->s_id.b.domain, sess->s_id.b.area, sess->s_id.b.al_pa,
+                        s_id.b.domain, s_id.b.area, s_id.b.al_pa);
+
+        if (sess->loop_id != loop_id) {
+                /*
+                 * Because we can shuffle loop IDs around and we
+                 * update different sessions non-atomically, we might
+                 * have overwritten this session's old loop ID
+                 * already, and we might end up overwriting some other
+                 * session that will be updated later.  So we have to
+                 * be extra careful and we can't warn about those things...
+                 */
+                if (lport->lport_loopid_map[sess->loop_id].se_nacl == se_nacl)
+                        lport->lport_loopid_map[sess->loop_id].se_nacl = NULL;
+
+                lport->lport_loopid_map[loop_id].se_nacl = se_nacl;
+
+                sess->loop_id = loop_id;
+        }
+
+        if (sess->s_id.b24 != s_id.b24) {
+                key = (((u32) sess->s_id.b.domain << 16) |
+                       ((u32) sess->s_id.b.area   <<  8) |
+                       ((u32) sess->s_id.b.al_pa));
+
+                if (btree_lookup32(&lport->lport_fcport_map, key))
+                        WARN(btree_remove32(&lport->lport_fcport_map, key) != se_nacl,
+                             "Found wrong se_nacl when updating s_id %x:%x:%x\n",
+                             sess->s_id.b.domain, sess->s_id.b.area, sess->s_id.b.al_pa);
+                else
+                        WARN(1, "No lport_fcport_map entry for s_id %x:%x:%x\n",
+                             sess->s_id.b.domain, sess->s_id.b.area, sess->s_id.b.al_pa);
+
+                key = (((u32) s_id.b.domain << 16) |
+                       ((u32) s_id.b.area   <<  8) |
+                       ((u32) s_id.b.al_pa));
+
+                if (btree_lookup32(&lport->lport_fcport_map, key)) {
+                        WARN(1, "Already have lport_fcport_map entry for s_id %x:%x:%x\n",
+                             s_id.b.domain, s_id.b.area, s_id.b.al_pa);
+                        btree_update32(&lport->lport_fcport_map, key, se_nacl);
+                } else {
+                        btree_insert32(&lport->lport_fcport_map, key, se_nacl, GFP_ATOMIC);
+                }
+
+                sess->s_id = s_id;
+                nacl->nport_id = key;
+        }
+
+        sess->conf_compl_supported = conf_compl_supported;
+}
+
 /*
  * Calls into tcm_qla2xxx used by qla2xxx LLD I/O path.
  */
@@ -1478,6 +1539,7 @@ static struct qla_tgt_func_tmpl tcm_qla2xxx_template = {
         .free_cmd               = tcm_qla2xxx_free_cmd,
         .free_mcmd              = tcm_qla2xxx_free_mcmd,
         .free_session           = tcm_qla2xxx_free_session,
+        .update_sess            = tcm_qla2xxx_update_sess,
         .check_initiator_node_acl = tcm_qla2xxx_check_initiator_node_acl,
         .find_sess_by_s_id      = tcm_qla2xxx_find_sess_by_s_id,
         .find_sess_by_loop_id   = tcm_qla2xxx_find_sess_by_loop_id,
@@ -1545,6 +1607,7 @@ static struct se_wwn *tcm_qla2xxx_make_lport(
         lport->lport_wwpn = wwpn;
         tcm_qla2xxx_format_wwn(&lport->lport_name[0], TCM_QLA2XXX_NAMELEN,
                                 wwpn);
+        sprintf(lport->lport_naa_name, "naa.%016llx", (unsigned long long) wwpn);
 
         ret = tcm_qla2xxx_init_lport(lport);
         if (ret != 0)
@@ -1612,6 +1675,7 @@ static struct se_wwn *tcm_qla2xxx_npiv_make_lport(
         lport->lport_npiv_wwnn = npiv_wwnn;
         tcm_qla2xxx_npiv_format_wwn(&lport->lport_npiv_name[0],
                         TCM_QLA2XXX_NAMELEN, npiv_wwpn, npiv_wwnn);
+        sprintf(lport->lport_naa_name, "naa.%016llx", (unsigned long long) npiv_wwpn);
 
 /* FIXME: tcm_qla2xxx_npiv_make_lport */
         ret = -ENOSYS;
@@ -1691,8 +1755,6 @@ static struct target_core_fabric_ops tcm_qla2xxx_ops = {
         .queue_data_in                  = tcm_qla2xxx_queue_data_in,
         .queue_status                   = tcm_qla2xxx_queue_status,
         .queue_tm_rsp                   = tcm_qla2xxx_queue_tm_rsp,
-        .get_fabric_sense_len           = tcm_qla2xxx_get_fabric_sense_len,
-        .set_fabric_sense_len           = tcm_qla2xxx_set_fabric_sense_len,
         /*
          * Setup function pointers for generic logic in
          * target_core_fabric_configfs.c
@@ -1740,8 +1802,6 @@ static struct target_core_fabric_ops tcm_qla2xxx_npiv_ops = {
         .queue_data_in                  = tcm_qla2xxx_queue_data_in,
         .queue_status                   = tcm_qla2xxx_queue_status,
         .queue_tm_rsp                   = tcm_qla2xxx_queue_tm_rsp,
-        .get_fabric_sense_len           = tcm_qla2xxx_get_fabric_sense_len,
-        .set_fabric_sense_len           = tcm_qla2xxx_set_fabric_sense_len,
         /*
          * Setup function pointers for generic logic in
          * target_core_fabric_configfs.c
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.h b/drivers/scsi/qla2xxx/tcm_qla2xxx.h
index 825498103352..9ba075fe9781 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.h
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.h
@@ -61,6 +61,8 @@ struct tcm_qla2xxx_lport {
         u64 lport_npiv_wwnn;
         /* ASCII formatted WWPN for FC Target Lport */
         char lport_name[TCM_QLA2XXX_NAMELEN];
+        /* ASCII formatted naa WWPN for VPD page 83 etc */
+        char lport_naa_name[TCM_QLA2XXX_NAMELEN];
         /* ASCII formatted WWPN+WWNN for NPIV FC Target Lport */
         char lport_npiv_name[TCM_QLA2XXX_NPIV_NAMELEN];
         /* map for fc_port pointers in 24-bit FC Port ID space */
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index 57fbd5a3d4e2..5cda11c07c68 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -2055,7 +2055,7 @@ static void unmap_region(sector_t lba, unsigned int len)
                 block = lba + alignment;
                 rem = do_div(block, granularity);
 
-                if (rem == 0 && lba + granularity <= end && block < map_size) {
+                if (rem == 0 && lba + granularity < end && block < map_size) {
                         clear_bit(block, map_storep);
                         if (scsi_debug_lbprz)
                                 memset(fake_storep +
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index de2337f255a7..c1b05a83d403 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -789,7 +789,6 @@ static int scsi_send_eh_cmnd(struct scsi_cmnd *scmd, unsigned char *cmnd,
                              int cmnd_size, int timeout, unsigned sense_bytes)
 {
         struct scsi_device *sdev = scmd->device;
-        struct scsi_driver *sdrv = scsi_cmd_to_driver(scmd);
         struct Scsi_Host *shost = sdev->host;
         DECLARE_COMPLETION_ONSTACK(done);
         unsigned long timeleft;
@@ -845,8 +844,11 @@ static int scsi_send_eh_cmnd(struct scsi_cmnd *scmd, unsigned char *cmnd,
 
         scsi_eh_restore_cmnd(scmd, &ses);
 
-        if (sdrv && sdrv->eh_action)
-                rtn = sdrv->eh_action(scmd, cmnd, cmnd_size, rtn);
+        if (scmd->request->cmd_type != REQ_TYPE_BLOCK_PC) {
+                struct scsi_driver *sdrv = scsi_cmd_to_driver(scmd);
+                if (sdrv->eh_action)
+                        rtn = sdrv->eh_action(scmd, cmnd, cmnd_size, rtn);
+        }
 
         return rtn;
 }
diff --git a/drivers/scsi/storvsc_drv.c b/drivers/scsi/storvsc_drv.c
index 528d52beaa1c..01440782feb2 100644
--- a/drivers/scsi/storvsc_drv.c
+++ b/drivers/scsi/storvsc_drv.c
@@ -1221,7 +1221,12 @@ static int storvsc_host_reset_handler(struct scsi_cmnd *scmnd)
         /*
          * At this point, all outstanding requests in the adapter
          * should have been flushed out and return to us
+         * There is a potential race here where the host may be in
+         * the process of responding when we return from here.
+         * Just wait for all in-transit packets to be accounted for
+         * before we return from here.
          */
+        storvsc_wait_to_drain(stor_device);
 
         return SUCCESS;
 }
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index 3e79a2f00042..595af1ae4421 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -219,7 +219,7 @@ static int virtscsi_kick_event(struct virtio_scsi *vscsi,
         struct scatterlist sg;
         unsigned long flags;
 
-        sg_set_buf(&sg, &event_node->event, sizeof(struct virtio_scsi_event));
+        sg_init_one(&sg, &event_node->event, sizeof(struct virtio_scsi_event));
 
         spin_lock_irqsave(&vscsi->event_vq.vq_lock, flags);
 
@@ -279,6 +279,31 @@ static void virtscsi_handle_transport_reset(struct virtio_scsi *vscsi,
         }
 }
 
+static void virtscsi_handle_param_change(struct virtio_scsi *vscsi,
+                                         struct virtio_scsi_event *event)
+{
+        struct scsi_device *sdev;
+        struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
+        unsigned int target = event->lun[1];
+        unsigned int lun = (event->lun[2] << 8) | event->lun[3];
+        u8 asc = event->reason & 255;
+        u8 ascq = event->reason >> 8;
+
+        sdev = scsi_device_lookup(shost, 0, target, lun);
+        if (!sdev) {
+                pr_err("SCSI device %d 0 %d %d not found\n",
+                        shost->host_no, target, lun);
+                return;
+        }
+
+        /* Handle "Parameters changed", "Mode parameters changed", and
+           "Capacity data has changed".  */
+        if (asc == 0x2a && (ascq == 0x00 || ascq == 0x01 || ascq == 0x09))
+                scsi_rescan_device(&sdev->sdev_gendev);
+
+        scsi_device_put(sdev);
+}
+
 static void virtscsi_handle_event(struct work_struct *work)
 {
         struct virtio_scsi_event_node *event_node =
@@ -297,6 +322,9 @@ static void virtscsi_handle_event(struct work_struct *work)
         case VIRTIO_SCSI_T_TRANSPORT_RESET:
                 virtscsi_handle_transport_reset(vscsi, event);
                 break;
+        case VIRTIO_SCSI_T_PARAM_CHANGE:
+                virtscsi_handle_param_change(vscsi, event);
+                break;
         default:
                 pr_err("Unsupport virtio scsi event %x\n", event->event);
         }
@@ -677,7 +705,11 @@ static int __devinit virtscsi_probe(struct virtio_device *vdev)
         cmd_per_lun = virtscsi_config_get(vdev, cmd_per_lun) ?: 1;
         shost->cmd_per_lun = min_t(u32, cmd_per_lun, shost->can_queue);
         shost->max_sectors = virtscsi_config_get(vdev, max_sectors) ?: 0xFFFF;
-        shost->max_lun = virtscsi_config_get(vdev, max_lun) + 1;
+
+        /* LUNs > 256 are reported with format 1, so they go in the range
+         * 16640-32767.
+         */
+        shost->max_lun = virtscsi_config_get(vdev, max_lun) + 1 + 0x4000;
         shost->max_id = num_targets;
         shost->max_channel = 0;
         shost->max_cmd_len = VIRTIO_SCSI_CDB_SIZE;
@@ -733,7 +765,8 @@ static struct virtio_device_id id_table[] = {
 };
 
 static unsigned int features[] = {
-        VIRTIO_SCSI_F_HOTPLUG
+        VIRTIO_SCSI_F_HOTPLUG,
+        VIRTIO_SCSI_F_CHANGE,
 };
 
 static struct virtio_driver virtio_scsi_driver = {
diff --git a/drivers/sh/intc/access.c b/drivers/sh/intc/access.c
index f892ae1d212a..114390f967d2 100644
--- a/drivers/sh/intc/access.c
+++ b/drivers/sh/intc/access.c
@@ -75,54 +75,61 @@ unsigned long intc_get_field_from_handle(unsigned int value, unsigned int handle
 static unsigned long test_8(unsigned long addr, unsigned long h,
                             unsigned long ignore)
 {
-        return intc_get_field_from_handle(__raw_readb(addr), h);
+        void __iomem *ptr = (void __iomem *)addr;
+        return intc_get_field_from_handle(__raw_readb(ptr), h);
 }
 
 static unsigned long test_16(unsigned long addr, unsigned long h,
                              unsigned long ignore)
 {
-        return intc_get_field_from_handle(__raw_readw(addr), h);
+        void __iomem *ptr = (void __iomem *)addr;
+        return intc_get_field_from_handle(__raw_readw(ptr), h);
 }
 
 static unsigned long test_32(unsigned long addr, unsigned long h,
                              unsigned long ignore)
 {
-        return intc_get_field_from_handle(__raw_readl(addr), h);
+        void __iomem *ptr = (void __iomem *)addr;
+        return intc_get_field_from_handle(__raw_readl(ptr), h);
 }
 
 static unsigned long write_8(unsigned long addr, unsigned long h,
                              unsigned long data)
 {
-        __raw_writeb(intc_set_field_from_handle(0, data, h), addr);
-        (void)__raw_readb(addr);        /* Defeat write posting */
+        void __iomem *ptr = (void __iomem *)addr;
+        __raw_writeb(intc_set_field_from_handle(0, data, h), ptr);
+        (void)__raw_readb(ptr); /* Defeat write posting */
         return 0;
 }
 
 static unsigned long write_16(unsigned long addr, unsigned long h,
                               unsigned long data)
 {
-        __raw_writew(intc_set_field_from_handle(0, data, h), addr);
-        (void)__raw_readw(addr);        /* Defeat write posting */
+        void __iomem *ptr = (void __iomem *)addr;
+        __raw_writew(intc_set_field_from_handle(0, data, h), ptr);
+        (void)__raw_readw(ptr); /* Defeat write posting */
         return 0;
 }
 
 static unsigned long write_32(unsigned long addr, unsigned long h,
                               unsigned long data)
 {
-        __raw_writel(intc_set_field_from_handle(0, data, h), addr);
-        (void)__raw_readl(addr);        /* Defeat write posting */
+        void __iomem *ptr = (void __iomem *)addr;
+        __raw_writel(intc_set_field_from_handle(0, data, h), ptr);
+        (void)__raw_readl(ptr); /* Defeat write posting */
         return 0;
 }
 
 static unsigned long modify_8(unsigned long addr, unsigned long h,
                               unsigned long data)
 {
+        void __iomem *ptr = (void __iomem *)addr;
         unsigned long flags;
         unsigned int value;
         local_irq_save(flags);
-        value = intc_set_field_from_handle(__raw_readb(addr), data, h);
-        __raw_writeb(value, addr);
-        (void)__raw_readb(addr);        /* Defeat write posting */
+        value = intc_set_field_from_handle(__raw_readb(ptr), data, h);
+        __raw_writeb(value, ptr);
+        (void)__raw_readb(ptr); /* Defeat write posting */
         local_irq_restore(flags);
         return 0;
 }
@@ -130,12 +137,13 @@ static unsigned long modify_8(unsigned long addr, unsigned long h,
 static unsigned long modify_16(unsigned long addr, unsigned long h,
                                unsigned long data)
 {
+        void __iomem *ptr = (void __iomem *)addr;
         unsigned long flags;
         unsigned int value;
         local_irq_save(flags);
-        value = intc_set_field_from_handle(__raw_readw(addr), data, h);
-        __raw_writew(value, addr);
-        (void)__raw_readw(addr);        /* Defeat write posting */
+        value = intc_set_field_from_handle(__raw_readw(ptr), data, h);
+        __raw_writew(value, ptr);
+        (void)__raw_readw(ptr); /* Defeat write posting */
         local_irq_restore(flags);
         return 0;
 }
@@ -143,12 +151,13 @@ static unsigned long modify_16(unsigned long addr, unsigned long h,
 static unsigned long modify_32(unsigned long addr, unsigned long h,
                                unsigned long data)
 {
+        void __iomem *ptr = (void __iomem *)addr;
         unsigned long flags;
         unsigned int value;
         local_irq_save(flags);
-        value = intc_set_field_from_handle(__raw_readl(addr), data, h);
-        __raw_writel(value, addr);
-        (void)__raw_readl(addr);        /* Defeat write posting */
+        value = intc_set_field_from_handle(__raw_readl(ptr), data, h);
+        __raw_writel(value, ptr);
+        (void)__raw_readl(ptr); /* Defeat write posting */
         local_irq_restore(flags);
         return 0;
 }
diff --git a/drivers/sh/intc/chip.c b/drivers/sh/intc/chip.c
index 012df2676a26..46427b48e2f1 100644
--- a/drivers/sh/intc/chip.c
+++ b/drivers/sh/intc/chip.c
@@ -83,7 +83,7 @@ static void intc_mask_ack(struct irq_data *data)
         unsigned int irq = data->irq;
         struct intc_desc_int *d = get_intc_desc(irq);
         unsigned long handle = intc_get_ack_handle(irq);
-        unsigned long addr;
+        void __iomem *addr;
 
         intc_disable(data);
 
@@ -91,7 +91,7 @@ static void intc_mask_ack(struct irq_data *data)
         if (handle) {
                 unsigned int value;
 
-                addr = INTC_REG(d, _INTC_ADDR_D(handle), 0);
+                addr = (void __iomem *)INTC_REG(d, _INTC_ADDR_D(handle), 0);
                 value = intc_set_field_from_handle(0, 1, handle);
 
                 switch (_INTC_FN(handle)) {
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 8c2ff2490d99..1acae359cabe 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -134,6 +134,7 @@ config SPI_DAVINCI
         tristate "Texas Instruments DaVinci/DA8x/OMAP-L/AM1x SoC SPI controller"
         depends on ARCH_DAVINCI
         select SPI_BITBANG
+        select TI_EDMA
         help
           SPI master controller for DaVinci/DA8x/OMAP-L/AM1x SPI modules.
 
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 3afe2f4f5b8e..147dfa87a64b 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -25,13 +25,14 @@
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/clk.h>
+#include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
+#include <linux/edma.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
 #include <linux/slab.h>
 
 #include <linux/platform_data/spi-davinci.h>
-#include <mach/edma.h>
 
 #define SPI_NO_RESOURCE         ((resource_size_t)-1)
 
@@ -113,14 +114,6 @@
 #define SPIDEF          0x4c
 #define SPIFMT0         0x50
 
-/* We have 2 DMA channels per CS, one for RX and one for TX */
-struct davinci_spi_dma {
-        int                     tx_channel;
-        int                     rx_channel;
-        int                     dummy_param_slot;
-        enum dma_event_q        eventq;
-};
-
 /* SPI Controller driver's private data. */
 struct davinci_spi {
         struct spi_bitbang      bitbang;
@@ -134,11 +127,14 @@ struct davinci_spi {
 
         const void              *tx;
         void                    *rx;
-#define SPI_TMP_BUFSZ   (SMP_CACHE_BYTES + 1)
-        u8                      rx_tmp_buf[SPI_TMP_BUFSZ];
         int                     rcount;
         int                     wcount;
-        struct davinci_spi_dma  dma;
+
+        struct dma_chan         *dma_rx;
+        struct dma_chan         *dma_tx;
+        int                     dma_rx_chnum;
+        int                     dma_tx_chnum;
+
         struct davinci_spi_platform_data *pdata;
 
         void                    (*get_rx)(u32 rx_data, struct davinci_spi *);
@@ -496,21 +492,23 @@ out:
         return errors;
 }
 
-static void davinci_spi_dma_callback(unsigned lch, u16 status, void *data)
+static void davinci_spi_dma_rx_callback(void *data)
 {
-        struct davinci_spi *dspi = data;
-        struct davinci_spi_dma *dma = &dspi->dma;
+        struct davinci_spi *dspi = (struct davinci_spi *)data;
 
-        edma_stop(lch);
+        dspi->rcount = 0;
 
-        if (status == DMA_COMPLETE) {
-                if (lch == dma->rx_channel)
-                        dspi->rcount = 0;
-                if (lch == dma->tx_channel)
-                        dspi->wcount = 0;
-        }
+        if (!dspi->wcount && !dspi->rcount)
+                complete(&dspi->done);
+}
 
-        if ((!dspi->wcount && !dspi->rcount) || (status != DMA_COMPLETE))
+static void davinci_spi_dma_tx_callback(void *data)
+{
+        struct davinci_spi *dspi = (struct davinci_spi *)data;
+
+        dspi->wcount = 0;
+
+        if (!dspi->wcount && !dspi->rcount)
                 complete(&dspi->done);
 }
 
@@ -526,20 +524,20 @@ static void davinci_spi_dma_callback(unsigned lch, u16 status, void *data)
 static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 {
         struct davinci_spi *dspi;
-        int data_type, ret;
+        int data_type, ret = -ENOMEM;
         u32 tx_data, spidat1;
         u32 errors = 0;
         struct davinci_spi_config *spicfg;
         struct davinci_spi_platform_data *pdata;
         unsigned uninitialized_var(rx_buf_count);
-        struct device *sdev;
+        void *dummy_buf = NULL;
+        struct scatterlist sg_rx, sg_tx;
 
         dspi = spi_master_get_devdata(spi->master);
         pdata = dspi->pdata;
         spicfg = (struct davinci_spi_config *)spi->controller_data;
         if (!spicfg)
                 spicfg = &davinci_spi_default_cfg;
-        sdev = dspi->bitbang.master->dev.parent;
 
         /* convert len to words based on bits_per_word */
         data_type = dspi->bytes_per_word[spi->chip_select];
@@ -567,112 +565,83 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
                 spidat1 |= tx_data & 0xFFFF;
                 iowrite32(spidat1, dspi->base + SPIDAT1);
         } else {
-                struct davinci_spi_dma *dma;
-                unsigned long tx_reg, rx_reg;
-                struct edmacc_param param;
-                void *rx_buf;
-                int b, c;
-
-                dma = &dspi->dma;
-
-                tx_reg = (unsigned long)dspi->pbase + SPIDAT1;
-                rx_reg = (unsigned long)dspi->pbase + SPIBUF;
-
-                /*
-                 * Transmit DMA setup
-                 *
-                 * If there is transmit data, map the transmit buffer, set it
-                 * as the source of data and set the source B index to data
-                 * size. If there is no transmit data, set the transmit register
-                 * as the source of data, and set the source B index to zero.
-                 *
-                 * The destination is always the transmit register itself. And
-                 * the destination never increments.
-                 */
-
-                if (t->tx_buf) {
-                        t->tx_dma = dma_map_single(&spi->dev, (void *)t->tx_buf,
-                                                t->len, DMA_TO_DEVICE);
-                        if (dma_mapping_error(&spi->dev, t->tx_dma)) {
-                                dev_dbg(sdev, "Unable to DMA map %d bytes"
-                                                "TX buffer\n", t->len);
-                                return -ENOMEM;
-                        }
-                }
-
-                /*
-                 * If number of words is greater than 65535, then we need
-                 * to configure a 3 dimension transfer.  Use the BCNTRLD
-                 * feature to allow for transfers that aren't even multiples
-                 * of 65535 (or any other possible b size) by first transferring
-                 * the remainder amount then grabbing the next N blocks of
-                 * 65535 words.
-                 */
-
-                c = dspi->wcount / (SZ_64K - 1);        /* N 65535 Blocks */
-                b = dspi->wcount - c * (SZ_64K - 1);    /* Remainder */
-                if (b)
-                        c++;
+                struct dma_slave_config dma_rx_conf = {
+                        .direction = DMA_DEV_TO_MEM,
+                        .src_addr = (unsigned long)dspi->pbase + SPIBUF,
+                        .src_addr_width = data_type,
+                        .src_maxburst = 1,
+                };
+                struct dma_slave_config dma_tx_conf = {
+                        .direction = DMA_MEM_TO_DEV,
+                        .dst_addr = (unsigned long)dspi->pbase + SPIDAT1,
+                        .dst_addr_width = data_type,
+                        .dst_maxburst = 1,
+                };
+                struct dma_async_tx_descriptor *rxdesc;
+                struct dma_async_tx_descriptor *txdesc;
+                void *buf;
+
+                dummy_buf = kzalloc(t->len, GFP_KERNEL);
+                if (!dummy_buf)
+                        goto err_alloc_dummy_buf;
+
+                dmaengine_slave_config(dspi->dma_rx, &dma_rx_conf);
+                dmaengine_slave_config(dspi->dma_tx, &dma_tx_conf);
+
+                sg_init_table(&sg_rx, 1);
+                if (!t->rx_buf)
+                        buf = dummy_buf;
                 else
-                        b = SZ_64K - 1;
-
-                param.opt = TCINTEN | EDMA_TCC(dma->tx_channel);
-                param.src = t->tx_buf ? t->tx_dma : tx_reg;
-                param.a_b_cnt = b << 16 | data_type;
-                param.dst = tx_reg;
-                param.src_dst_bidx = t->tx_buf ? data_type : 0;
-                param.link_bcntrld = 0xffffffff;
-                param.src_dst_cidx = t->tx_buf ? data_type : 0;
-                param.ccnt = c;
-                edma_write_slot(dma->tx_channel, &param);
-                edma_link(dma->tx_channel, dma->dummy_param_slot);
-
-                /*
-                 * Receive DMA setup
-                 *
-                 * If there is receive buffer, use it to receive data. If there
-                 * is none provided, use a temporary receive buffer. Set the
-                 * destination B index to 0 so effectively only one byte is used
-                 * in the temporary buffer (address does not increment).
-                 *
-                 * The source of receive data is the receive data register. The
-                 * source address never increments.
-                 */
-
-                if (t->rx_buf) {
-                        rx_buf = t->rx_buf;
-                        rx_buf_count = t->len;
-                } else {
-                        rx_buf = dspi->rx_tmp_buf;
-                        rx_buf_count = sizeof(dspi->rx_tmp_buf);
+                        buf = t->rx_buf;
+                t->rx_dma = dma_map_single(&spi->dev, buf,
+                                t->len, DMA_FROM_DEVICE);
+                if (!t->rx_dma) {
+                        ret = -EFAULT;
+                        goto err_rx_map;
                 }
+                sg_dma_address(&sg_rx) = t->rx_dma;
+                sg_dma_len(&sg_rx) = t->len;
 
-                t->rx_dma = dma_map_single(&spi->dev, rx_buf, rx_buf_count,
-                                                        DMA_FROM_DEVICE);
-                if (dma_mapping_error(&spi->dev, t->rx_dma)) {
-                        dev_dbg(sdev, "Couldn't DMA map a %d bytes RX buffer\n",
-                                                                rx_buf_count);
-                        if (t->tx_buf)
-                                dma_unmap_single(&spi->dev, t->tx_dma, t->len,
-                                                                DMA_TO_DEVICE);
-                        return -ENOMEM;
+                sg_init_table(&sg_tx, 1);
+                if (!t->tx_buf)
+                        buf = dummy_buf;
+                else
+                        buf = (void *)t->tx_buf;
+                t->tx_dma = dma_map_single(&spi->dev, buf,
+                                t->len, DMA_FROM_DEVICE);
+                if (!t->tx_dma) {
+                        ret = -EFAULT;
+                        goto err_tx_map;
                 }
-
-                param.opt = TCINTEN | EDMA_TCC(dma->rx_channel);
-                param.src = rx_reg;
-                param.a_b_cnt = b << 16 | data_type;
-                param.dst = t->rx_dma;
-                param.src_dst_bidx = (t->rx_buf ? data_type : 0) << 16;
-                param.link_bcntrld = 0xffffffff;
-                param.src_dst_cidx = (t->rx_buf ? data_type : 0) << 16;
-                param.ccnt = c;
-                edma_write_slot(dma->rx_channel, &param);
+                sg_dma_address(&sg_tx) = t->tx_dma;
+                sg_dma_len(&sg_tx) = t->len;
+
+                rxdesc = dmaengine_prep_slave_sg(dspi->dma_rx,
+                                &sg_rx, 1, DMA_DEV_TO_MEM,
+                                DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!rxdesc)
+                        goto err_desc;
+
+                txdesc = dmaengine_prep_slave_sg(dspi->dma_tx,
+                                &sg_tx, 1, DMA_MEM_TO_DEV,
+                                DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+                if (!txdesc)
+                        goto err_desc;
+
+                rxdesc->callback = davinci_spi_dma_rx_callback;
+                rxdesc->callback_param = (void *)dspi;
+                txdesc->callback = davinci_spi_dma_tx_callback;
+                txdesc->callback_param = (void *)dspi;
 
                 if (pdata->cshold_bug)
                         iowrite16(spidat1 >> 16, dspi->base + SPIDAT1 + 2);
 
-                edma_start(dma->rx_channel);
-                edma_start(dma->tx_channel);
+                dmaengine_submit(rxdesc);
+                dmaengine_submit(txdesc);
+
+                dma_async_issue_pending(dspi->dma_rx);
+                dma_async_issue_pending(dspi->dma_tx);
+
                 set_io_bits(dspi->base + SPIINT, SPIINT_DMA_REQ_EN);
         }
 
@@ -690,15 +659,13 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 
         clear_io_bits(dspi->base + SPIINT, SPIINT_MASKALL);
         if (spicfg->io_type == SPI_IO_TYPE_DMA) {
-
-                if (t->tx_buf)
-                        dma_unmap_single(&spi->dev, t->tx_dma, t->len,
-                                                                DMA_TO_DEVICE);
-
-                dma_unmap_single(&spi->dev, t->rx_dma, rx_buf_count,
-                                                        DMA_FROM_DEVICE);
-
                 clear_io_bits(dspi->base + SPIINT, SPIINT_DMA_REQ_EN);
+
+                dma_unmap_single(&spi->dev, t->rx_dma,
+                                t->len, DMA_FROM_DEVICE);
+                dma_unmap_single(&spi->dev, t->tx_dma,
+                                t->len, DMA_TO_DEVICE);
+                kfree(dummy_buf);
         }
 
         clear_io_bits(dspi->base + SPIGCR1, SPIGCR1_SPIENA_MASK);
@@ -716,11 +683,20 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
         }
 
         if (dspi->rcount != 0 || dspi->wcount != 0) {
-                dev_err(sdev, "SPI data transfer error\n");
+                dev_err(&spi->dev, "SPI data transfer error\n");
                 return -EIO;
         }
 
         return t->len;
+
+err_desc:
+        dma_unmap_single(&spi->dev, t->tx_dma, t->len, DMA_TO_DEVICE);
+err_tx_map:
+        dma_unmap_single(&spi->dev, t->rx_dma, t->len, DMA_FROM_DEVICE);
+err_rx_map:
+        kfree(dummy_buf);
+err_alloc_dummy_buf:
+        return ret;
 }
 
 /**
@@ -751,39 +727,33 @@ static irqreturn_t davinci_spi_irq(s32 irq, void *data)
 
 static int davinci_spi_request_dma(struct davinci_spi *dspi)
 {
+        dma_cap_mask_t mask;
+        struct device *sdev = dspi->bitbang.master->dev.parent;
         int r;
-        struct davinci_spi_dma *dma = &dspi->dma;
 
-        r = edma_alloc_channel(dma->rx_channel, davinci_spi_dma_callback, dspi,
-                                                                dma->eventq);
-        if (r < 0) {
-                pr_err("Unable to request DMA channel for SPI RX\n");
-                r = -EAGAIN;
+        dma_cap_zero(mask);
+        dma_cap_set(DMA_SLAVE, mask);
+
+        dspi->dma_rx = dma_request_channel(mask, edma_filter_fn,
+                                           &dspi->dma_rx_chnum);
+        if (!dspi->dma_rx) {
+                dev_err(sdev, "request RX DMA channel failed\n");
+                r = -ENODEV;
                 goto rx_dma_failed;
         }
 
-        r = edma_alloc_channel(dma->tx_channel, davinci_spi_dma_callback, dspi,
-                                                                dma->eventq);
-        if (r < 0) {
-                pr_err("Unable to request DMA channel for SPI TX\n");
-                r = -EAGAIN;
+        dspi->dma_tx = dma_request_channel(mask, edma_filter_fn,
+                                           &dspi->dma_tx_chnum);
+        if (!dspi->dma_tx) {
+                dev_err(sdev, "request TX DMA channel failed\n");
+                r = -ENODEV;
                 goto tx_dma_failed;
         }
 
-        r = edma_alloc_slot(EDMA_CTLR(dma->tx_channel), EDMA_SLOT_ANY);
-        if (r < 0) {
-                pr_err("Unable to request SPI TX DMA param slot\n");
-                r = -EAGAIN;
-                goto param_failed;
-        }
-        dma->dummy_param_slot = r;
-        edma_link(dma->dummy_param_slot, dma->dummy_param_slot);
-
         return 0;
-param_failed:
-        edma_free_channel(dma->tx_channel);
+
 tx_dma_failed:
-        edma_free_channel(dma->rx_channel);
+        dma_release_channel(dspi->dma_rx);
 rx_dma_failed:
         return r;
 }
@@ -898,9 +868,8 @@ static int __devinit davinci_spi_probe(struct platform_device *pdev)
         dspi->bitbang.txrx_bufs = davinci_spi_bufs;
         if (dma_rx_chan != SPI_NO_RESOURCE &&
             dma_tx_chan != SPI_NO_RESOURCE) {
-                dspi->dma.rx_channel = dma_rx_chan;
-                dspi->dma.tx_channel = dma_tx_chan;
-                dspi->dma.eventq = pdata->dma_event_q;
+                dspi->dma_rx_chnum = dma_rx_chan;
+                dspi->dma_tx_chnum = dma_tx_chan;
 
                 ret = davinci_spi_request_dma(dspi);
                 if (ret)
@@ -955,9 +924,8 @@ static int __devinit davinci_spi_probe(struct platform_device *pdev)
         return ret;
 
 free_dma:
-        edma_free_channel(dspi->dma.tx_channel);
-        edma_free_channel(dspi->dma.rx_channel);
-        edma_free_slot(dspi->dma.dummy_param_slot);
+        dma_release_channel(dspi->dma_rx);
+        dma_release_channel(dspi->dma_tx);
 free_clk:
         clk_disable(dspi->clk);
         clk_put(dspi->clk);
diff --git a/drivers/spi/spi-mxs.c b/drivers/spi/spi-mxs.c
index edf1360ab09e..86dd04d6bc87 100644
--- a/drivers/spi/spi-mxs.c
+++ b/drivers/spi/spi-mxs.c
@@ -323,6 +323,7 @@ static int mxs_spi_txrx_dma(struct mxs_spi *spi, int cs,
         if (!ret) {
                 dev_err(ssp->dev, "DMA transfer timeout\n");
                 ret = -ETIMEDOUT;
+                dmaengine_terminate_all(ssp->dmach);
                 goto err_vmalloc;
         }
 
@@ -480,7 +481,7 @@ static int mxs_spi_transfer_one(struct spi_master *master,
                 first = last = 0;
         }
 
-        m->status = 0;
+        m->status = status;
         spi_finalize_current_message(master);
 
         return status;
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 919464102d33..a1db91a99b89 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -2186,8 +2186,6 @@ pl022_probe(struct amba_device *adev, const struct amba_id *id)
         printk(KERN_INFO "pl022: mapped registers from 0x%08x to %p\n",
                adev->res.start, pl022->virtbase);
 
-        pm_runtime_resume(dev);
-
         pl022->clk = devm_clk_get(&adev->dev, NULL);
         if (IS_ERR(pl022->clk)) {
                 status = PTR_ERR(pl022->clk);
@@ -2292,7 +2290,6 @@ pl022_remove(struct amba_device *adev)
 
         clk_disable(pl022->clk);
         clk_unprepare(pl022->clk);
-        pm_runtime_disable(&adev->dev);
         amba_release_regions(adev);
         tasklet_disable(&pl022->pump_transfers);
         spi_unregister_master(pl022->master);
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index 4894bde4bbff..30faf6d4ab91 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -147,8 +147,6 @@ struct rspi_data {
         unsigned char spsr;
 
         /* for dmaengine */
-        struct sh_dmae_slave dma_tx;
-        struct sh_dmae_slave dma_rx;
         struct dma_chan *chan_tx;
         struct dma_chan *chan_rx;
         int irq;
@@ -663,20 +661,16 @@ static irqreturn_t rspi_irq(int irq, void *_sr)
         return ret;
 }
 
-static bool rspi_filter(struct dma_chan *chan, void *filter_param)
-{
-        chan->private = filter_param;
-        return true;
-}
-
-static void __devinit rspi_request_dma(struct rspi_data *rspi,
-                                       struct platform_device *pdev)
+static int __devinit rspi_request_dma(struct rspi_data *rspi,
+                                      struct platform_device *pdev)
 {
         struct rspi_plat_data *rspi_pd = pdev->dev.platform_data;
         dma_cap_mask_t mask;
+        struct dma_slave_config cfg;
+        int ret;
 
         if (!rspi_pd)
-                return;
+                return 0;       /* The driver assumes no error. */
 
         rspi->dma_width_16bit = rspi_pd->dma_width_16bit;
 
@@ -684,21 +678,35 @@ static void __devinit rspi_request_dma(struct rspi_data *rspi,
         if (rspi_pd->dma_rx_id && rspi_pd->dma_tx_id) {
                 dma_cap_zero(mask);
                 dma_cap_set(DMA_SLAVE, mask);
-                rspi->dma_rx.slave_id = rspi_pd->dma_rx_id;
-                rspi->chan_rx = dma_request_channel(mask, rspi_filter,
-                                                    &rspi->dma_rx);
-                if (rspi->chan_rx)
-                        dev_info(&pdev->dev, "Use DMA when rx.\n");
+                rspi->chan_rx = dma_request_channel(mask, shdma_chan_filter,
+                                                    (void *)rspi_pd->dma_rx_id);
+                if (rspi->chan_rx) {
+                        cfg.slave_id = rspi_pd->dma_rx_id;
+                        cfg.direction = DMA_DEV_TO_MEM;
+                        ret = dmaengine_slave_config(rspi->chan_rx, &cfg);
+                        if (!ret)
+                                dev_info(&pdev->dev, "Use DMA when rx.\n");
+                        else
+                                return ret;
+                }
         }
         if (rspi_pd->dma_tx_id) {
                 dma_cap_zero(mask);
                 dma_cap_set(DMA_SLAVE, mask);
-                rspi->dma_tx.slave_id = rspi_pd->dma_tx_id;
-                rspi->chan_tx = dma_request_channel(mask, rspi_filter,
-                                                    &rspi->dma_tx);
-                if (rspi->chan_tx)
-                        dev_info(&pdev->dev, "Use DMA when tx\n");
+                rspi->chan_tx = dma_request_channel(mask, shdma_chan_filter,
+                                                    (void *)rspi_pd->dma_tx_id);
+                if (rspi->chan_tx) {
+                        cfg.slave_id = rspi_pd->dma_tx_id;
+                        cfg.direction = DMA_MEM_TO_DEV;
+                        ret = dmaengine_slave_config(rspi->chan_tx, &cfg);
+                        if (!ret)
+                                dev_info(&pdev->dev, "Use DMA when tx\n");
+                        else
+                                return ret;
+                }
         }
+
+        return 0;
 }
 
 static void __devexit rspi_release_dma(struct rspi_data *rspi)
@@ -788,7 +796,11 @@ static int __devinit rspi_probe(struct platform_device *pdev)
         }
 
         rspi->irq = irq;
-        rspi_request_dma(rspi, pdev);
+        ret = rspi_request_dma(rspi, pdev);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "rspi_request_dma failed.\n");
+                goto error4;
+        }
 
         ret = spi_register_master(master);
         if (ret < 0) {
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index 1a81c90a4a71..6e7a805d324d 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -132,7 +132,7 @@
 
 struct s3c64xx_spi_dma_data {
         unsigned                ch;
-        enum dma_data_direction direction;
+        enum dma_transfer_direction direction;
         enum dma_ch     dmach;
         struct property         *dma_prop;
 };
@@ -1067,11 +1067,11 @@ static int __devinit s3c64xx_spi_get_dmares(
 
         if (tx) {
                 dma_data = &sdd->tx_dma;
-                dma_data->direction = DMA_TO_DEVICE;
+                dma_data->direction = DMA_MEM_TO_DEV;
                 chan_str = "tx";
         } else {
                 dma_data = &sdd->rx_dma;
-                dma_data->direction = DMA_FROM_DEVICE;
+                dma_data->direction = DMA_DEV_TO_MEM;
                 chan_str = "rx";
         }
 
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index 7b0ba92e7e46..5d4610babd8a 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -567,7 +567,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
                 page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
 
                 BUG_ON(*page);
-                *page = alloc_page(GFP_KERNEL | __GFP_ZERO);
+                *page = alloc_page(GFP_KERNEL | __GFP_HIGHMEM | __GFP_ZERO);
                 if (*page == NULL) {
                         pr_err("binder: %d: binder_alloc_buf failed "
                                "for page at %p\n", proc->pid, page_addr);
@@ -2419,14 +2419,38 @@ static void binder_release_work(struct list_head *list)
                         struct binder_transaction *t;
 
                         t = container_of(w, struct binder_transaction, work);
-                        if (t->buffer->target_node && !(t->flags & TF_ONE_WAY))
+                        if (t->buffer->target_node &&
+                            !(t->flags & TF_ONE_WAY)) {
                                 binder_send_failed_reply(t, BR_DEAD_REPLY);
+                        } else {
+                                binder_debug(BINDER_DEBUG_DEAD_TRANSACTION,
+                                        "binder: undelivered transaction %d\n",
+                                        t->debug_id);
+                                t->buffer->transaction = NULL;
+                                kfree(t);
+                                binder_stats_deleted(BINDER_STAT_TRANSACTION);
+                        }
                 } break;
                 case BINDER_WORK_TRANSACTION_COMPLETE: {
+                        binder_debug(BINDER_DEBUG_DEAD_TRANSACTION,
+                                "binder: undelivered TRANSACTION_COMPLETE\n");
                         kfree(w);
                         binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
                 } break;
+                case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+                case BINDER_WORK_CLEAR_DEATH_NOTIFICATION: {
+                        struct binder_ref_death *death;
+
+                        death = container_of(w, struct binder_ref_death, work);
+                        binder_debug(BINDER_DEBUG_DEAD_TRANSACTION,
+                                "binder: undelivered death notification, %p\n",
+                                death->cookie);
+                        kfree(death);
+                        binder_stats_deleted(BINDER_STAT_DEATH);
+                } break;
                 default:
+                        pr_err("binder: unexpected work type, %d, not freed\n",
+                               w->type);
                         break;
                 }
         }
@@ -2899,6 +2923,7 @@ static void binder_deferred_release(struct binder_proc *proc)
                 nodes++;
                 rb_erase(&node->rb_node, &proc->nodes);
                 list_del_init(&node->work.entry);
+                binder_release_work(&node->async_todo);
                 if (hlist_empty(&node->refs)) {
                         kfree(node);
                         binder_stats_deleted(BINDER_STAT_NODE);
@@ -2937,6 +2962,7 @@ static void binder_deferred_release(struct binder_proc *proc)
                 binder_delete_ref(ref);
         }
         binder_release_work(&proc->todo);
+        binder_release_work(&proc->delivered_death);
         buffers = 0;
 
         while ((n = rb_first(&proc->allocated_buffers))) {
diff --git a/drivers/staging/comedi/drivers/8255_pci.c b/drivers/staging/comedi/drivers/8255_pci.c
index 7dff3c01dc29..d00aff6671df 100644
--- a/drivers/staging/comedi/drivers/8255_pci.c
+++ b/drivers/staging/comedi/drivers/8255_pci.c
@@ -289,6 +289,8 @@ static void pci_8255_detach(struct comedi_device *dev)
         struct comedi_subdevice *s;
         int i;
 
+        if (!board || !devpriv)
+                return;
         if (dev->subdevices) {
                 for (i = 0; i < board->n_8255; i++) {
                         s = &dev->subdevices[i];
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 08f305210a69..29eb52d11d2f 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -1410,6 +1410,8 @@ static void dio200_detach(struct comedi_device *dev)
         const struct dio200_layout_struct *layout;
         unsigned n;
 
+        if (!thisboard)
+                return;
         if (dev->irq)
                 free_irq(dev->irq, dev);
         if (dev->subdevices) {
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index eacb5e4735d7..4e4f3c15df87 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -573,9 +573,10 @@ static int __devinit pc236_attach_pci(struct comedi_device *dev,
 static void pc236_detach(struct comedi_device *dev)
 {
         const struct pc236_board *thisboard = comedi_board(dev);
-        struct pc236_private *devpriv = dev->private;
 
-        if (devpriv)
+        if (!thisboard)
+                return;
+        if (dev->iobase)
                 pc236_intr_disable(dev);
         if (dev->irq)
                 free_irq(dev->irq, dev);
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index 60830ccfb903..d0a4c441228b 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -323,6 +323,8 @@ static void pc263_detach(struct comedi_device *dev)
 {
         const struct pc263_board *thisboard = comedi_board(dev);
 
+        if (!thisboard)
+                return;
         if (is_isa_board(thisboard)) {
                 if (dev->iobase)
                         release_region(dev->iobase, PC263_IO_SIZE);
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index 5fd21fa6c1c7..c304528cfb13 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -846,6 +846,8 @@ static void __maybe_unused das08_detach(struct comedi_device *dev)
 {
         const struct das08_board_struct *thisboard = comedi_board(dev);
 
+        if (!thisboard)
+                return;
         das08_common_detach(dev);
         if (is_isa_board(thisboard)) {
                 if (dev->iobase)
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index 2ba0ade45c64..68d7c6a5db7d 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -95,7 +95,7 @@ static int daq700_dio_insn_bits(struct comedi_device *dev,
         }
 
         data[1] = s->state & 0xff;
-        data[1] |= inb(dev->iobase + DIO_R);
+        data[1] |= inb(dev->iobase + DIO_R) << 8;
 
         return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index 28b91a6c3789..b5a19a0863fb 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -772,6 +772,8 @@ void labpc_common_detach(struct comedi_device *dev)
 {
         struct comedi_subdevice *s;
 
+        if (!thisboard)
+                return;
         if (dev->subdevices) {
                 s = &dev->subdevices[2];
                 subdev_8255_cleanup(dev, s);
diff --git a/drivers/staging/dgrp/dgrp_mon_ops.c b/drivers/staging/dgrp/dgrp_mon_ops.c
index 268dcb95204b..4792d056a365 100644
--- a/drivers/staging/dgrp/dgrp_mon_ops.c
+++ b/drivers/staging/dgrp/dgrp_mon_ops.c
@@ -38,6 +38,7 @@
 #include <linux/sched.h>
 #include <asm/unaligned.h>
 #include <linux/proc_fs.h>
+#include <linux/uaccess.h>
 
 #include "dgrp_common.h"
 
diff --git a/drivers/staging/dgrp/dgrp_specproc.c b/drivers/staging/dgrp/dgrp_specproc.c
index 28f5c9ab6b43..24327c3bad83 100644
--- a/drivers/staging/dgrp/dgrp_specproc.c
+++ b/drivers/staging/dgrp/dgrp_specproc.c
@@ -39,6 +39,7 @@
 #include <linux/proc_fs.h>
 #include <linux/ctype.h>
 #include <linux/seq_file.h>
+#include <linux/uaccess.h>
 #include <linux/vmalloc.h>
 
 #include "dgrp_common.h"
@@ -228,6 +229,9 @@ static void register_proc_table(struct dgrp_proc_entry *table,
         int len;
         mode_t mode;
 
+        if (table == NULL)
+                return;
+
         for (; table->id; table++) {
                 /* Can't do anything without a proc name. */
                 if (!table->name)
@@ -296,6 +300,9 @@ static void unregister_proc_table(struct dgrp_proc_entry *table,
         struct proc_dir_entry *de;
         struct nd_struct *tmp;
 
+        if (table == NULL)
+                return;
+
         list_for_each_entry(tmp, &nd_struct_list, list) {
                 if ((table == dgrp_net_table) && (tmp->nd_net_de)) {
                         unregister_dgrp_device(tmp->nd_net_de);
diff --git a/drivers/staging/dgrp/dgrp_tty.c b/drivers/staging/dgrp/dgrp_tty.c
index 7d7de873870c..e125b03598d7 100644
--- a/drivers/staging/dgrp/dgrp_tty.c
+++ b/drivers/staging/dgrp/dgrp_tty.c
@@ -40,6 +40,7 @@
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
 #include <linux/sched.h>
+#include <linux/uaccess.h>
 
 #include "dgrp_common.h"
 
@@ -3172,6 +3173,9 @@ dgrp_tty_init(struct nd_struct *nd)
          */
 
         nd->nd_serial_ttdriver = alloc_tty_driver(CHAN_MAX);
+        if (!nd->nd_serial_ttdriver)
+                return -ENOMEM;
+
         sprintf(nd->nd_serial_name,  "tty_dgrp_%s_", id);
 
         nd->nd_serial_ttdriver->owner = THIS_MODULE;
@@ -3231,6 +3235,9 @@ dgrp_tty_init(struct nd_struct *nd)
         }
 
         nd->nd_callout_ttdriver = alloc_tty_driver(CHAN_MAX);
+        if (!nd->nd_callout_ttdriver)
+                return -ENOMEM;
+
         sprintf(nd->nd_callout_name, "cu_dgrp_%s_",  id);
 
         nd->nd_callout_ttdriver->owner = THIS_MODULE;
@@ -3268,6 +3275,9 @@ dgrp_tty_init(struct nd_struct *nd)
 
 
         nd->nd_xprint_ttdriver = alloc_tty_driver(CHAN_MAX);
+        if (!nd->nd_xprint_ttdriver)
+                return -ENOMEM;
+
         sprintf(nd->nd_xprint_name,  "pr_dgrp_%s_", id);
 
         nd->nd_xprint_ttdriver->owner = THIS_MODULE;
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 8e37d6e04277..b12ca68cd9e4 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -310,30 +310,32 @@ static int adis16201_read_raw(struct iio_dev *indio_dev,
         case IIO_CHAN_INFO_SCALE:
                 switch (chan->type) {
                 case IIO_VOLTAGE:
-                        *val = 0;
-                        if (chan->channel == 0)
-                                *val2 = 1220;
-                        else
-                                *val2 = 610;
+                        if (chan->channel == 0) {
+                                *val = 1;
+                                *val2 = 220000; /* 1.22 mV */
+                        } else {
+                                *val = 0;
+                                *val2 = 610000; /* 0.610 mV */
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = -470000;
+                        *val = -470; /* 0.47 C */
+                        *val2 = 0;
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_ACCEL:
                         *val = 0;
-                        *val2 = 462500;
-                        return IIO_VAL_INT_PLUS_MICRO;
+                        *val2 = IIO_G_TO_M_S_2(462400); /* 0.4624 mg */
+                        return IIO_VAL_INT_PLUS_NANO;
                 case IIO_INCLI:
                         *val = 0;
-                        *val2 = 100000;
+                        *val2 = 100000; /* 0.1 degree */
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
                 }
                 break;
         case IIO_CHAN_INFO_OFFSET:
-                *val = 25;
+                *val = 25000 / -470 - 1278; /* 25 C = 1278 */
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_CALIBBIAS:
                 switch (chan->type) {
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index 002fa9dfc375..e7b3441115ae 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -316,25 +316,27 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
         case IIO_CHAN_INFO_SCALE:
                 switch (chan->type) {
                 case IIO_VOLTAGE:
-                        *val = 0;
-                        if (chan->channel == 0)
-                                *val2 = 1220;
-                        else
-                                *val2 = 610;
+                        if (chan->channel == 0) {
+                                *val = 1;
+                                *val2 = 220000; /* 1.22 mV */
+                        } else {
+                                *val = 0;
+                                *val2 = 610000; /* 0.61 mV */
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = -470000;
+                        *val = -470; /* -0.47 C */
+                        *val2 = 0;
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_INCLI:
                         *val = 0;
-                        *val2 = 25000;
+                        *val2 = 25000; /* 0.025 degree */
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
                 }
         case IIO_CHAN_INFO_OFFSET:
-                *val = 25;
+                *val = 25000 / -470 - 1278; /* 25 C = 1278 */
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_CALIBBIAS:
                 bits = 14;
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index 05bdb7c2c8e3..c6234c2f46aa 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -317,26 +317,28 @@ static int adis16204_read_raw(struct iio_dev *indio_dev,
         case IIO_CHAN_INFO_SCALE:
                 switch (chan->type) {
                 case IIO_VOLTAGE:
-                        *val = 0;
-                        if (chan->channel == 0)
-                                *val2 = 1220;
-                        else
-                                *val2 = 610;
+                        if (chan->channel == 0) {
+                                *val = 1;
+                                *val2 = 220000; /* 1.22 mV */
+                        } else {
+                                *val = 0;
+                                *val2 = 610000; /* 0.61 mV */
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = -470000;
+                        *val = -470; /* 0.47 C */
+                        *val2 = 0;
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_ACCEL:
                         *val = 0;
                         switch (chan->channel2) {
                         case IIO_MOD_X:
                         case IIO_MOD_ROOT_SUM_SQUARED_X_Y:
-                                *val2 = 17125;
+                                *val2 = IIO_G_TO_M_S_2(17125); /* 17.125 mg */
                                 break;
                         case IIO_MOD_Y:
                         case IIO_MOD_Z:
-                                *val2 = 8407;
+                                *val2 = IIO_G_TO_M_S_2(8407); /* 8.407 mg */
                                 break;
                         }
                         return IIO_VAL_INT_PLUS_MICRO;
@@ -345,7 +347,7 @@ static int adis16204_read_raw(struct iio_dev *indio_dev,
                 }
                 break;
         case IIO_CHAN_INFO_OFFSET:
-                *val = 25;
+                *val = 25000 / -470 - 1278; /* 25 C = 1278 */
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_CALIBBIAS:
         case IIO_CHAN_INFO_PEAK:
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index b7333bfe0b2f..7ee974b45d7d 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -343,28 +343,29 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
                 case IIO_VOLTAGE:
                         *val = 0;
                         if (chan->channel == 0)
-                                *val2 = 305180;
+                                *val2 = 305180; /* 0.30518 mV */
                         else
-                                *val2 = 610500;
+                                *val2 = 610500; /* 0.6105 mV */
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = -470000;
+                        *val = -470; /* -0.47 C */
+                        *val2 = 0;
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_ACCEL:
                         *val = 0;
-                        *val2 = 2394;
-                        return IIO_VAL_INT_PLUS_MICRO;
+                        *val2 = IIO_G_TO_M_S_2(244140); /* 0.244140 mg */
+                        return IIO_VAL_INT_PLUS_NANO;
                 case IIO_INCLI:
+                case IIO_ROT:
                         *val = 0;
-                        *val2 = 436;
+                        *val2 = 25000; /* 0.025 degree */
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
                 }
                 break;
         case IIO_CHAN_INFO_OFFSET:
-                *val = 25;
+                *val = 25000 / -470 - 0x4FE; /* 25 C = 0x4FE */
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_CALIBBIAS:
                 switch (chan->type) {
@@ -491,6 +492,7 @@ static const struct iio_chan_spec adis16209_channels[] = {
                 .modified = 1,
                 .channel2 = IIO_MOD_X,
                 .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+                IIO_CHAN_INFO_SCALE_SHARED_BIT,
                 .address = rot,
                 .scan_index = ADIS16209_SCAN_ROT,
                 .scan_type = {
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index c755089c7117..eaadd9df3f78 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -486,7 +486,7 @@ static int adis16220_read_raw(struct iio_dev *indio_dev,
                 break;
         case IIO_CHAN_INFO_OFFSET:
                 if (chan->type == IIO_TEMP) {
-                        *val = 25;
+                        *val = 25000 / -470 - 1278; /* 25 C = 1278 */
                         return IIO_VAL_INT;
                 }
                 addrind = 1;
@@ -495,19 +495,22 @@ static int adis16220_read_raw(struct iio_dev *indio_dev,
                 addrind = 2;
                 break;
         case IIO_CHAN_INFO_SCALE:
-                *val = 0;
                 switch (chan->type) {
                 case IIO_TEMP:
-                        *val2 = -470000;
+                        *val = -470; /* -0.47 C */
+                        *val2 = 0;
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_ACCEL:
-                        *val2 = 1887042;
+                        *val2 = IIO_G_TO_M_S_2(19073); /* 19.073 g */
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_VOLTAGE:
-                        if (chan->channel == 0)
-                                *val2 = 0012221;
-                        else /* Should really be dependent on VDD */
-                                *val2 = 305;
+                        if (chan->channel == 0) {
+                                *val = 1;
+                                *val2 = 220700; /* 1.2207 mV */
+                        } else {
+                                /* Should really be dependent on VDD */
+                                *val2 = 305180; /* 305.18 uV */
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 0fc26a49d681..35e093973d5c 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -373,30 +373,31 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
         case IIO_CHAN_INFO_SCALE:
                 switch (chan->type) {
                 case IIO_VOLTAGE:
-                        *val = 0;
-                        if (chan->channel == 0)
-                                *val2 = 4880;
-                        else
+                        if (chan->channel == 0) {
+                                *val = 4;
+                                *val2 = 880000; /* 4.88 mV */
+                                return IIO_VAL_INT_PLUS_MICRO;
+                        } else {
                                 return -EINVAL;
-                        return IIO_VAL_INT_PLUS_MICRO;
+                        }
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = 244000;
+                        *val = 244; /* 0.244 C */
+                        *val2 = 0;
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_ACCEL:
                         *val = 0;
-                        *val2 = 504062;
+                        *val2 = IIO_G_TO_M_S_2(51400); /* 51.4 mg */
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
                 }
                 break;
         case IIO_CHAN_INFO_PEAK_SCALE:
-                *val = 6;
-                *val2 = 629295;
+                *val = 0;
+                *val2 = IIO_G_TO_M_S_2(51400); /* 51.4 mg */
                 return IIO_VAL_INT_PLUS_MICRO;
         case IIO_CHAN_INFO_OFFSET:
-                *val = 25;
+                *val = 25000 / 244 - 0x133; /* 25 C = 0x133 */
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_CALIBBIAS:
                 bits = 10;
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index 9571c03aa4cc..aa964a2d8290 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -498,28 +498,33 @@ static int adis16260_read_raw(struct iio_dev *indio_dev,
                 switch (chan->type) {
                 case IIO_ANGL_VEL:
                         *val = 0;
-                        if (spi_get_device_id(st->us)->driver_data)
-                                *val2 = 320;
-                        else
-                                *val2 = 1278;
+                        if (spi_get_device_id(st->us)->driver_data) {
+                                /* 0.01832 degree / sec */
+                                *val2 = IIO_DEGREE_TO_RAD(18320);
+                        } else {
+                                /* 0.07326 degree / sec */
+                                *val2 = IIO_DEGREE_TO_RAD(73260);
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_VOLTAGE:
-                        *val = 0;
-                        if (chan->channel == 0)
-                                *val2 = 18315;
-                        else
-                                *val2 = 610500;
+                        if (chan->channel == 0) {
+                                *val = 1;
+                                *val2 = 831500; /* 1.8315 mV */
+                        } else {
+                                *val = 0;
+                                *val2 = 610500; /* 610.5 uV */
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = 145300;
+                        *val = 145;
+                        *val2 = 300000; /* 0.1453 C */
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
                 }
                 break;
         case IIO_CHAN_INFO_OFFSET:
-                *val = 25;
+                *val = 250000 / 1453; /* 25 C = 0x00 */
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_CALIBBIAS:
                 switch (chan->type) {
diff --git a/drivers/staging/iio/imu/adis16400.h b/drivers/staging/iio/imu/adis16400.h
index d59d7ac856a9..77c601da1846 100644
--- a/drivers/staging/iio/imu/adis16400.h
+++ b/drivers/staging/iio/imu/adis16400.h
@@ -139,6 +139,8 @@ struct adis16400_chip_info {
         const long flags;
         unsigned int gyro_scale_micro;
         unsigned int accel_scale_micro;
+        int temp_scale_nano;
+        int temp_offset;
         unsigned long default_scan_mask;
 };
 
diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c
index b302c9ba2712..3144a7b1e1c4 100644
--- a/drivers/staging/iio/imu/adis16400_core.c
+++ b/drivers/staging/iio/imu/adis16400_core.c
@@ -553,10 +553,13 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_VOLTAGE:
                         *val = 0;
-                        if (chan->channel == 0)
-                                *val2 = 2418;
-                        else
-                                *val2 = 806;
+                        if (chan->channel == 0) {
+                                *val = 2;
+                                *val2 = 418000; /* 2.418 mV */
+                        } else {
+                                *val = 0;
+                                *val2 = 805800; /* 805.8 uV */
+                        }
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_ACCEL:
                         *val = 0;
@@ -564,11 +567,11 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_MAGN:
                         *val = 0;
-                        *val2 = 500;
+                        *val2 = 500; /* 0.5 mgauss */
                         return IIO_VAL_INT_PLUS_MICRO;
                 case IIO_TEMP:
-                        *val = 0;
-                        *val2 = 140000;
+                        *val = st->variant->temp_scale_nano / 1000000;
+                        *val2 = (st->variant->temp_scale_nano % 1000000);
                         return IIO_VAL_INT_PLUS_MICRO;
                 default:
                         return -EINVAL;
@@ -586,9 +589,8 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
                 return IIO_VAL_INT;
         case IIO_CHAN_INFO_OFFSET:
                 /* currently only temperature */
-                *val = 198;
-                *val2 = 160000;
-                return IIO_VAL_INT_PLUS_MICRO;
+                *val = st->variant->temp_offset;
+                return IIO_VAL_INT;
         case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
                 mutex_lock(&indio_dev->mlock);
                 /* Need both the number of taps and the sampling frequency */
@@ -1035,7 +1037,7 @@ static const struct iio_chan_spec adis16334_channels[] = {
                 .indexed = 1,
                 .channel = 0,
                 .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-                IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+                IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
                 IIO_CHAN_INFO_SCALE_SHARED_BIT,
                 .address = temp0,
                 .scan_index = ADIS16400_SCAN_TEMP,
@@ -1058,8 +1060,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
         [ADIS16300] = {
                 .channels = adis16300_channels,
                 .num_channels = ARRAY_SIZE(adis16300_channels),
-                .gyro_scale_micro = 873,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
                 .accel_scale_micro = 5884,
+                .temp_scale_nano = 140000000, /* 0.14 C */
+                .temp_offset = 25000000 / 140000, /* 25 C = 0x00 */
                 .default_scan_mask = (1 << ADIS16400_SCAN_SUPPLY) |
                 (1 << ADIS16400_SCAN_GYRO_X) | (1 << ADIS16400_SCAN_ACC_X) |
                 (1 << ADIS16400_SCAN_ACC_Y) | (1 << ADIS16400_SCAN_ACC_Z) |
@@ -1070,8 +1074,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
         [ADIS16334] = {
                 .channels = adis16334_channels,
                 .num_channels = ARRAY_SIZE(adis16334_channels),
-                .gyro_scale_micro = 873,
-                .accel_scale_micro = 981,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */
+                .temp_scale_nano = 67850000, /* 0.06785 C */
+                .temp_offset = 25000000 / 67850, /* 25 C = 0x00 */
                 .default_scan_mask = (1 << ADIS16400_SCAN_GYRO_X) |
                 (1 << ADIS16400_SCAN_GYRO_Y) | (1 << ADIS16400_SCAN_GYRO_Z) |
                 (1 << ADIS16400_SCAN_ACC_X) | (1 << ADIS16400_SCAN_ACC_Y) |
@@ -1080,8 +1086,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
         [ADIS16350] = {
                 .channels = adis16350_channels,
                 .num_channels = ARRAY_SIZE(adis16350_channels),
-                .gyro_scale_micro = 872664,
-                .accel_scale_micro = 24732,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(73260), /* 0.07326 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(2522), /* 0.002522 g */
+                .temp_scale_nano = 145300000, /* 0.1453 C */
+                .temp_offset = 25000000 / 145300, /* 25 C = 0x00 */
                 .default_scan_mask = 0x7FF,
                 .flags = ADIS16400_NO_BURST,
         },
@@ -1090,8 +1098,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
                 .num_channels = ARRAY_SIZE(adis16350_channels),
                 .flags = ADIS16400_HAS_PROD_ID,
                 .product_id = 0x3FE8,
-                .gyro_scale_micro = 1279,
-                .accel_scale_micro = 24732,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */
+                .temp_scale_nano = 136000000, /* 0.136 C */
+                .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */
                 .default_scan_mask = 0x7FF,
         },
         [ADIS16362] = {
@@ -1099,8 +1109,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
                 .num_channels = ARRAY_SIZE(adis16350_channels),
                 .flags = ADIS16400_HAS_PROD_ID,
                 .product_id = 0x3FEA,
-                .gyro_scale_micro = 1279,
-                .accel_scale_micro = 24732,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(333), /* 0.333 mg */
+                .temp_scale_nano = 136000000, /* 0.136 C */
+                .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */
                 .default_scan_mask = 0x7FF,
         },
         [ADIS16364] = {
@@ -1108,8 +1120,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
                 .num_channels = ARRAY_SIZE(adis16350_channels),
                 .flags = ADIS16400_HAS_PROD_ID,
                 .product_id = 0x3FEC,
-                .gyro_scale_micro = 1279,
-                .accel_scale_micro = 24732,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */
+                .temp_scale_nano = 136000000, /* 0.136 C */
+                .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */
                 .default_scan_mask = 0x7FF,
         },
         [ADIS16365] = {
@@ -1117,8 +1131,10 @@ static struct adis16400_chip_info adis16400_chips[] = {
                 .num_channels = ARRAY_SIZE(adis16350_channels),
                 .flags = ADIS16400_HAS_PROD_ID,
                 .product_id = 0x3FED,
-                .gyro_scale_micro = 1279,
-                .accel_scale_micro = 24732,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */
+                .temp_scale_nano = 136000000, /* 0.136 C */
+                .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */
                 .default_scan_mask = 0x7FF,
         },
         [ADIS16400] = {
@@ -1126,9 +1142,11 @@ static struct adis16400_chip_info adis16400_chips[] = {
                 .num_channels = ARRAY_SIZE(adis16400_channels),
                 .flags = ADIS16400_HAS_PROD_ID,
                 .product_id = 0x4015,
-                .gyro_scale_micro = 873,
-                .accel_scale_micro = 32656,
+                .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */
+                .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */
                 .default_scan_mask = 0xFFF,
+                .temp_scale_nano = 140000000, /* 0.14 C */
+                .temp_offset = 25000000 / 140000, /* 25 C = 0x00 */
         }
 };
 
diff --git a/drivers/staging/ipack/bridges/tpci200.c b/drivers/staging/ipack/bridges/tpci200.c
index bb8aa70281cd..46d6657280b8 100644
--- a/drivers/staging/ipack/bridges/tpci200.c
+++ b/drivers/staging/ipack/bridges/tpci200.c
@@ -12,6 +12,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/slab.h>
 #include "tpci200.h"
 
 static u16 tpci200_status_timeout[] = {
diff --git a/drivers/staging/omap-thermal/omap-thermal-common.c b/drivers/staging/omap-thermal/omap-thermal-common.c
index 46ee0a9f49d9..5c0c203b887f 100644
--- a/drivers/staging/omap-thermal/omap-thermal-common.c
+++ b/drivers/staging/omap-thermal/omap-thermal-common.c
@@ -126,7 +126,9 @@ static int omap_thermal_bind(struct thermal_zone_device *thermal,
 
         /* TODO: bind with min and max states */
         /* Simple thing, two trips, one passive another critical */
-        return thermal_zone_bind_cooling_device(thermal, 0, cdev);
+        return thermal_zone_bind_cooling_device(thermal, 0, cdev,
+                                                THERMAL_NO_LIMIT,
+                                                THERMAL_NO_LIMIT);
 }
 
 /* Unbind callback functions for thermal zone */
@@ -268,7 +270,6 @@ int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
         /* Create thermal zone */
         data->omap_thermal = thermal_zone_device_register(domain,
                                 OMAP_TRIP_NUMBER, 0, data, &omap_thermal_ops,
-                                1, 2, /*TODO: remove this when FW allows */
                                 FAST_TEMP_MONITORING_RATE,
                                 FAST_TEMP_MONITORING_RATE);
         if (IS_ERR_OR_NULL(data->omap_thermal)) {
diff --git a/drivers/staging/omapdrm/omap_drv.c b/drivers/staging/omapdrm/omap_drv.c
index 2ec5264dd002..ebdb0b676737 100644
--- a/drivers/staging/omapdrm/omap_drv.c
+++ b/drivers/staging/omapdrm/omap_drv.c
@@ -106,7 +106,8 @@ static void dump_video_chains(void)
         for (i = 0; i < omap_dss_get_num_overlays(); i++) {
                 struct omap_overlay *ovl = omap_dss_get_overlay(i);
                 struct omap_overlay_manager *mgr = ovl->manager;
-                struct omap_dss_device *dssdev = mgr ? mgr->device : NULL;
+                struct omap_dss_device *dssdev = mgr ?
+                                        mgr->get_device(mgr) : NULL;
                 if (dssdev) {
                         DBG("%d: %s -> %s -> %s", i, ovl->name, mgr->name,
                                                 dssdev->name);
@@ -185,7 +186,7 @@ static int create_connector(struct drm_device *dev,
         for (j = 0; j < priv->num_encoders; j++) {
                 struct omap_overlay_manager *mgr =
                         omap_encoder_get_manager(priv->encoders[j]);
-                if (mgr->device == dssdev) {
+                if (mgr->get_device(mgr) == dssdev) {
                         drm_mode_connector_attach_encoder(connector,
                                         priv->encoders[j]);
                 }
diff --git a/drivers/staging/omapdrm/omap_gem.c b/drivers/staging/omapdrm/omap_gem.c
index 3434e6ec0142..66e2c2f8a239 100644
--- a/drivers/staging/omapdrm/omap_gem.c
+++ b/drivers/staging/omapdrm/omap_gem.c
@@ -246,7 +246,7 @@ static int omap_gem_attach_pages(struct drm_gem_object *obj)
          * DSS, GPU, etc. are not cache coherent:
          */
         if (omap_obj->flags & (OMAP_BO_WC|OMAP_BO_UNCACHED)) {
-                addrs = kmalloc(npages * sizeof(addrs), GFP_KERNEL);
+                addrs = kmalloc(npages * sizeof(*addrs), GFP_KERNEL);
                 if (!addrs) {
                         ret = -ENOMEM;
                         goto free_pages;
@@ -257,7 +257,7 @@ static int omap_gem_attach_pages(struct drm_gem_object *obj)
                                         0, PAGE_SIZE, DMA_BIDIRECTIONAL);
                 }
         } else {
-                addrs = kzalloc(npages * sizeof(addrs), GFP_KERNEL);
+                addrs = kzalloc(npages * sizeof(*addrs), GFP_KERNEL);
                 if (!addrs) {
                         ret = -ENOMEM;
                         goto free_pages;
diff --git a/drivers/staging/ramster/Kconfig b/drivers/staging/ramster/Kconfig
index 843c54101438..3abf6619dace 100644
--- a/drivers/staging/ramster/Kconfig
+++ b/drivers/staging/ramster/Kconfig
@@ -18,6 +18,7 @@ config ZCACHE2
 config RAMSTER
         bool "Cross-machine RAM capacity sharing, aka peer-to-peer tmem"
         depends on CONFIGFS_FS=y && SYSFS=y && !HIGHMEM && ZCACHE2=y
+        depends on NET
         # must ensure struct page is 8-byte aligned
         select HAVE_ALIGNED_STRUCT_PAGE if !64_BIT
         default n
diff --git a/drivers/staging/tidspbridge/core/tiomap3430.c b/drivers/staging/tidspbridge/core/tiomap3430.c
index 066a3ceec65e..f619fb3c56d2 100644
--- a/drivers/staging/tidspbridge/core/tiomap3430.c
+++ b/drivers/staging/tidspbridge/core/tiomap3430.c
@@ -126,7 +126,8 @@ static int mem_map_vmalloc(struct bridge_dev_context *dev_context,
                                   u32 ul_num_bytes,
                                   struct hw_mmu_map_attrs_t *hw_attrs);
 
-bool wait_for_start(struct bridge_dev_context *dev_context, u32 dw_sync_addr);
+bool wait_for_start(struct bridge_dev_context *dev_context,
+                        void __iomem *sync_addr);
 
 /*  ----------------------------------- Globals */
 
@@ -363,10 +364,11 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 {
         int status = 0;
         struct bridge_dev_context *dev_context = dev_ctxt;
-        u32 dw_sync_addr = 0;
+        void __iomem *sync_addr;
         u32 ul_shm_base;        /* Gpp Phys SM base addr(byte) */
         u32 ul_shm_base_virt;   /* Dsp Virt SM base addr */
         u32 ul_tlb_base_virt;   /* Base of MMU TLB entry */
+        u32 shm_sync_pa;
         /* Offset of shm_base_virt from tlb_base_virt */
         u32 ul_shm_offset_virt;
         s32 entry_ndx;
@@ -397,15 +399,22 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
         /* Kernel logical address */
         ul_shm_base = dev_context->atlb_entry[0].gpp_va + ul_shm_offset_virt;
 
+        /* SHM physical sync address */
+        shm_sync_pa = dev_context->atlb_entry[0].gpp_pa + ul_shm_offset_virt +
+                        SHMSYNCOFFSET;
+
         /* 2nd wd is used as sync field */
-        dw_sync_addr = ul_shm_base + SHMSYNCOFFSET;
+        sync_addr = ioremap(shm_sync_pa, SZ_32);
+        if (!sync_addr)
+                return -ENOMEM;
+
         /* Write a signature into the shm base + offset; this will
          * get cleared when the DSP program starts. */
         if ((ul_shm_base_virt == 0) || (ul_shm_base == 0)) {
                 pr_err("%s: Illegal SM base\n", __func__);
                 status = -EPERM;
         } else
-                __raw_writel(0xffffffff, dw_sync_addr);
+                __raw_writel(0xffffffff, sync_addr);
 
         if (!status) {
                 resources = dev_context->resources;
@@ -419,8 +428,10 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
                          * function is made available.
                          */
                         void __iomem *ctrl = ioremap(0x48002000, SZ_4K);
-                        if (!ctrl)
+                        if (!ctrl) {
+                                iounmap(sync_addr);
                                 return -ENOMEM;
+                        }
 
                         (*pdata->dsp_prm_rmw_bits)(OMAP3430_RST1_IVA2_MASK,
                                         OMAP3430_RST1_IVA2_MASK, OMAP3430_IVA2_MOD,
@@ -588,15 +599,15 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
                 (*pdata->dsp_prm_rmw_bits)(OMAP3430_RST1_IVA2_MASK, 0,
                                         OMAP3430_IVA2_MOD, OMAP2_RM_RSTCTRL);
 
-                dev_dbg(bridge, "Waiting for Sync @ 0x%x\n", dw_sync_addr);
+                dev_dbg(bridge, "Waiting for Sync @ 0x%x\n", *(u32 *)sync_addr);
                 dev_dbg(bridge, "DSP c_int00 Address =  0x%x\n", dsp_addr);
                 if (dsp_debug)
-                        while (__raw_readw(dw_sync_addr))
+                        while (__raw_readw(sync_addr))
                                 ;
 
                 /* Wait for DSP to clear word in shared memory */
                 /* Read the Location */
-                if (!wait_for_start(dev_context, dw_sync_addr))
+                if (!wait_for_start(dev_context, sync_addr))
                         status = -ETIMEDOUT;
 
                 dev_get_symbol(dev_context->dev_obj, "_WDT_enable", &wdt_en);
@@ -612,7 +623,7 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
                         /* Write the synchronization bit to indicate the
                          * completion of OPP table update to DSP
                          */
-                        __raw_writel(0XCAFECAFE, dw_sync_addr);
+                        __raw_writel(0XCAFECAFE, sync_addr);
 
                         /* update board state */
                         dev_context->brd_state = BRD_RUNNING;
@@ -621,6 +632,9 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
                         dev_context->brd_state = BRD_UNKNOWN;
                 }
         }
+
+        iounmap(sync_addr);
+
         return status;
 }
 
@@ -1796,12 +1810,13 @@ static int mem_map_vmalloc(struct bridge_dev_context *dev_context,
  *  ======== wait_for_start ========
  *      Wait for the singal from DSP that it has started, or time out.
  */
-bool wait_for_start(struct bridge_dev_context *dev_context, u32 dw_sync_addr)
+bool wait_for_start(struct bridge_dev_context *dev_context,
+                        void __iomem *sync_addr)
 {
         u16 timeout = TIHELEN_ACKTIMEOUT;
 
         /*  Wait for response from board */
-        while (__raw_readw(dw_sync_addr) && --timeout)
+        while (__raw_readw(sync_addr) && --timeout)
                 udelay(10);
 
         /*  If timed out: return false */
diff --git a/drivers/staging/tidspbridge/hw/hw_mmu.c b/drivers/staging/tidspbridge/hw/hw_mmu.c
index 71cb82293649..50244a474178 100644
--- a/drivers/staging/tidspbridge/hw/hw_mmu.c
+++ b/drivers/staging/tidspbridge/hw/hw_mmu.c
@@ -48,37 +48,12 @@ enum hw_mmu_page_size_t {
 };
 
 /*
- * FUNCTION           : mmu_flush_entry
- *
- * INPUTS:
- *
- *       Identifier      : base_address
- *       Type           : const u32
- *       Description     : Base Address of instance of MMU module
- *
- * RETURNS:
- *
- *       Type           : hw_status
- *       Description     : 0             -- No errors occurred
- *                       RET_BAD_NULL_PARAM     -- A Pointer
- *                                              Parameter was set to NULL
- *
- * PURPOSE:           : Flush the TLB entry pointed by the
- *                      lock counter register
- *                      even if this entry is set protected
- *
- * METHOD:             : Check the Input parameter and Flush a
- *                       single entry in the TLB.
- */
-static hw_status mmu_flush_entry(const void __iomem *base_address);
-
-/*
  * FUNCTION           : mmu_set_cam_entry
  *
  * INPUTS:
  *
  *       Identifier      : base_address
- *       TypE           : const u32
+ *       Type            : void __iomem *
  *       Description     : Base Address of instance of MMU module
  *
  *       Identifier      : page_sz
@@ -112,7 +87,7 @@ static hw_status mmu_flush_entry(const void __iomem *base_address);
  *
  * METHOD:              : Check the Input parameters and set the CAM entry.
  */
-static hw_status mmu_set_cam_entry(const void __iomem *base_address,
+static hw_status mmu_set_cam_entry(void __iomem *base_address,
                                    const u32 page_sz,
                                    const u32 preserved_bit,
                                    const u32 valid_bit,
@@ -124,7 +99,7 @@ static hw_status mmu_set_cam_entry(const void __iomem *base_address,
  * INPUTS:
  *
  *       Identifier      : base_address
- *       Type           : const u32
+ *       Type            : void __iomem *
  *       Description     : Base Address of instance of MMU module
  *
  *       Identifier      : physical_addr
@@ -157,7 +132,7 @@ static hw_status mmu_set_cam_entry(const void __iomem *base_address,
  *
  * METHOD:             : Check the Input parameters and set the RAM entry.
  */
-static hw_status mmu_set_ram_entry(const void __iomem *base_address,
+static hw_status mmu_set_ram_entry(void __iomem *base_address,
                                    const u32 physical_addr,
                                    enum hw_endianism_t endianism,
                                    enum hw_element_size_t element_size,
@@ -165,7 +140,7 @@ static hw_status mmu_set_ram_entry(const void __iomem *base_address,
 
 /* HW FUNCTIONS */
 
-hw_status hw_mmu_enable(const void __iomem *base_address)
+hw_status hw_mmu_enable(void __iomem *base_address)
 {
         hw_status status = 0;
 
@@ -174,7 +149,7 @@ hw_status hw_mmu_enable(const void __iomem *base_address)
         return status;
 }
 
-hw_status hw_mmu_disable(const void __iomem *base_address)
+hw_status hw_mmu_disable(void __iomem *base_address)
 {
         hw_status status = 0;
 
@@ -183,7 +158,7 @@ hw_status hw_mmu_disable(const void __iomem *base_address)
         return status;
 }
 
-hw_status hw_mmu_num_locked_set(const void __iomem *base_address,
+hw_status hw_mmu_num_locked_set(void __iomem *base_address,
                                 u32 num_locked_entries)
 {
         hw_status status = 0;
@@ -193,7 +168,7 @@ hw_status hw_mmu_num_locked_set(const void __iomem *base_address,
         return status;
 }
 
-hw_status hw_mmu_victim_num_set(const void __iomem *base_address,
+hw_status hw_mmu_victim_num_set(void __iomem *base_address,
                                 u32 victim_entry_num)
 {
         hw_status status = 0;
@@ -203,7 +178,7 @@ hw_status hw_mmu_victim_num_set(const void __iomem *base_address,
         return status;
 }
 
-hw_status hw_mmu_event_ack(const void __iomem *base_address, u32 irq_mask)
+hw_status hw_mmu_event_ack(void __iomem *base_address, u32 irq_mask)
 {
         hw_status status = 0;
 
@@ -212,7 +187,7 @@ hw_status hw_mmu_event_ack(const void __iomem *base_address, u32 irq_mask)
         return status;
 }
 
-hw_status hw_mmu_event_disable(const void __iomem *base_address, u32 irq_mask)
+hw_status hw_mmu_event_disable(void __iomem *base_address, u32 irq_mask)
 {
         hw_status status = 0;
         u32 irq_reg;
@@ -224,7 +199,7 @@ hw_status hw_mmu_event_disable(const void __iomem *base_address, u32 irq_mask)
         return status;
 }
 
-hw_status hw_mmu_event_enable(const void __iomem *base_address, u32 irq_mask)
+hw_status hw_mmu_event_enable(void __iomem *base_address, u32 irq_mask)
 {
         hw_status status = 0;
         u32 irq_reg;
@@ -236,7 +211,7 @@ hw_status hw_mmu_event_enable(const void __iomem *base_address, u32 irq_mask)
         return status;
 }
 
-hw_status hw_mmu_event_status(const void __iomem *base_address, u32 *irq_mask)
+hw_status hw_mmu_event_status(void __iomem *base_address, u32 *irq_mask)
 {
         hw_status status = 0;
 
@@ -245,7 +220,7 @@ hw_status hw_mmu_event_status(const void __iomem *base_address, u32 *irq_mask)
         return status;
 }
 
-hw_status hw_mmu_fault_addr_read(const void __iomem *base_address, u32 *addr)
+hw_status hw_mmu_fault_addr_read(void __iomem *base_address, u32 *addr)
 {
         hw_status status = 0;
 
@@ -255,7 +230,7 @@ hw_status hw_mmu_fault_addr_read(const void __iomem *base_address, u32 *addr)
         return status;
 }
 
-hw_status hw_mmu_ttb_set(const void __iomem *base_address, u32 ttb_phys_addr)
+hw_status hw_mmu_ttb_set(void __iomem *base_address, u32 ttb_phys_addr)
 {
         hw_status status = 0;
         u32 load_ttb;
@@ -267,7 +242,7 @@ hw_status hw_mmu_ttb_set(const void __iomem *base_address, u32 ttb_phys_addr)
         return status;
 }
 
-hw_status hw_mmu_twl_enable(const void __iomem *base_address)
+hw_status hw_mmu_twl_enable(void __iomem *base_address)
 {
         hw_status status = 0;
 
@@ -276,7 +251,7 @@ hw_status hw_mmu_twl_enable(const void __iomem *base_address)
         return status;
 }
 
-hw_status hw_mmu_twl_disable(const void __iomem *base_address)
+hw_status hw_mmu_twl_disable(void __iomem *base_address)
 {
         hw_status status = 0;
 
@@ -285,45 +260,7 @@ hw_status hw_mmu_twl_disable(const void __iomem *base_address)
         return status;
 }
 
-hw_status hw_mmu_tlb_flush(const void __iomem *base_address, u32 virtual_addr,
-                           u32 page_sz)
-{
-        hw_status status = 0;
-        u32 virtual_addr_tag;
-        enum hw_mmu_page_size_t pg_size_bits;
-
-        switch (page_sz) {
-        case HW_PAGE_SIZE4KB:
-                pg_size_bits = HW_MMU_SMALL_PAGE;
-                break;
-
-        case HW_PAGE_SIZE64KB:
-                pg_size_bits = HW_MMU_LARGE_PAGE;
-                break;
-
-        case HW_PAGE_SIZE1MB:
-                pg_size_bits = HW_MMU_SECTION;
-                break;
-
-        case HW_PAGE_SIZE16MB:
-                pg_size_bits = HW_MMU_SUPERSECTION;
-                break;
-
-        default:
-                return -EINVAL;
-        }
-
-        /* Generate the 20-bit tag from virtual address */
-        virtual_addr_tag = ((virtual_addr & MMU_ADDR_MASK) >> 12);
-
-        mmu_set_cam_entry(base_address, pg_size_bits, 0, 0, virtual_addr_tag);
-
-        mmu_flush_entry(base_address);
-
-        return status;
-}
-
-hw_status hw_mmu_tlb_add(const void __iomem *base_address,
+hw_status hw_mmu_tlb_add(void __iomem *base_address,
                          u32 physical_addr,
                          u32 virtual_addr,
                          u32 page_sz,
@@ -503,20 +440,8 @@ hw_status hw_mmu_pte_clear(const u32 pg_tbl_va, u32 virtual_addr, u32 page_size)
         return status;
 }
 
-/* mmu_flush_entry */
-static hw_status mmu_flush_entry(const void __iomem *base_address)
-{
-        hw_status status = 0;
-        u32 flush_entry_data = 0x1;
-
-        /* write values to register */
-        MMUMMU_FLUSH_ENTRY_WRITE_REGISTER32(base_address, flush_entry_data);
-
-        return status;
-}
-
 /* mmu_set_cam_entry */
-static hw_status mmu_set_cam_entry(const void __iomem *base_address,
+static hw_status mmu_set_cam_entry(void __iomem *base_address,
                                    const u32 page_sz,
                                    const u32 preserved_bit,
                                    const u32 valid_bit,
@@ -536,7 +461,7 @@ static hw_status mmu_set_cam_entry(const void __iomem *base_address,
 }
 
 /* mmu_set_ram_entry */
-static hw_status mmu_set_ram_entry(const void __iomem *base_address,
+static hw_status mmu_set_ram_entry(void __iomem *base_address,
                                    const u32 physical_addr,
                                    enum hw_endianism_t endianism,
                                    enum hw_element_size_t element_size,
@@ -556,7 +481,7 @@ static hw_status mmu_set_ram_entry(const void __iomem *base_address,
 
 }
 
-void hw_mmu_tlb_flush_all(const void __iomem *base)
+void hw_mmu_tlb_flush_all(void __iomem *base)
 {
         __raw_writel(1, base + MMU_GFLUSH);
 }
diff --git a/drivers/staging/tidspbridge/hw/hw_mmu.h b/drivers/staging/tidspbridge/hw/hw_mmu.h
index 1458a2c6027b..1c50bb36edfe 100644
--- a/drivers/staging/tidspbridge/hw/hw_mmu.h
+++ b/drivers/staging/tidspbridge/hw/hw_mmu.h
@@ -42,44 +42,41 @@ struct hw_mmu_map_attrs_t {
         bool donotlockmpupage;
 };
 
-extern hw_status hw_mmu_enable(const void __iomem *base_address);
+extern hw_status hw_mmu_enable(void __iomem *base_address);
 
-extern hw_status hw_mmu_disable(const void __iomem *base_address);
+extern hw_status hw_mmu_disable(void __iomem *base_address);
 
-extern hw_status hw_mmu_num_locked_set(const void __iomem *base_address,
+extern hw_status hw_mmu_num_locked_set(void __iomem *base_address,
                                        u32 num_locked_entries);
 
-extern hw_status hw_mmu_victim_num_set(const void __iomem *base_address,
+extern hw_status hw_mmu_victim_num_set(void __iomem *base_address,
                                        u32 victim_entry_num);
 
 /* For MMU faults */
-extern hw_status hw_mmu_event_ack(const void __iomem *base_address,
+extern hw_status hw_mmu_event_ack(void __iomem *base_address,
                                   u32 irq_mask);
 
-extern hw_status hw_mmu_event_disable(const void __iomem *base_address,
+extern hw_status hw_mmu_event_disable(void __iomem *base_address,
                                       u32 irq_mask);
 
-extern hw_status hw_mmu_event_enable(const void __iomem *base_address,
+extern hw_status hw_mmu_event_enable(void __iomem *base_address,
                                      u32 irq_mask);
 
-extern hw_status hw_mmu_event_status(const void __iomem *base_address,
+extern hw_status hw_mmu_event_status(void __iomem *base_address,
                                      u32 *irq_mask);
 
-extern hw_status hw_mmu_fault_addr_read(const void __iomem *base_address,
+extern hw_status hw_mmu_fault_addr_read(void __iomem *base_address,
                                         u32 *addr);
 
 /* Set the TT base address */
-extern hw_status hw_mmu_ttb_set(const void __iomem *base_address,
+extern hw_status hw_mmu_ttb_set(void __iomem *base_address,
                                 u32 ttb_phys_addr);
 
-extern hw_status hw_mmu_twl_enable(const void __iomem *base_address);
+extern hw_status hw_mmu_twl_enable(void __iomem *base_address);
 
-extern hw_status hw_mmu_twl_disable(const void __iomem *base_address);
+extern hw_status hw_mmu_twl_disable(void __iomem *base_address);
 
-extern hw_status hw_mmu_tlb_flush(const void __iomem *base_address,
-                                  u32 virtual_addr, u32 page_sz);
-
-extern hw_status hw_mmu_tlb_add(const void __iomem *base_address,
+extern hw_status hw_mmu_tlb_add(void __iomem *base_address,
                                 u32 physical_addr,
                                 u32 virtual_addr,
                                 u32 page_sz,
@@ -97,7 +94,7 @@ extern hw_status hw_mmu_pte_set(const u32 pg_tbl_va,
 extern hw_status hw_mmu_pte_clear(const u32 pg_tbl_va,
                                   u32 virtual_addr, u32 page_size);
 
-void hw_mmu_tlb_flush_all(const void __iomem *base);
+void hw_mmu_tlb_flush_all(void __iomem *base);
 
 static inline u32 hw_mmu_pte_addr_l1(u32 l1_base, u32 va)
 {
diff --git a/drivers/staging/tidspbridge/include/dspbridge/cfgdefs.h b/drivers/staging/tidspbridge/include/dspbridge/cfgdefs.h
index 60a278136bdf..b32c75673ab4 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/cfgdefs.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/cfgdefs.h
@@ -53,8 +53,8 @@ struct cfg_hostres {
         u32 chnl_buf_size;
         u32 num_chnls;
         void __iomem *per_base;
-        u32 per_pm_base;
-        u32 core_pm_base;
+        void __iomem *per_pm_base;
+        void __iomem *core_pm_base;
         void __iomem *dmmu_base;
 };
 
diff --git a/drivers/staging/tidspbridge/include/dspbridge/host_os.h b/drivers/staging/tidspbridge/include/dspbridge/host_os.h
index ed00d3da3205..5e2f4d82d925 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/host_os.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/host_os.h
@@ -47,8 +47,8 @@
 #include <asm/cacheflush.h>
 #include <linux/dma-mapping.h>
 
-/* TODO -- Remove, once BP defines them */
-#define INT_DSP_MMU_IRQ        28
+/* TODO -- Remove, once omap-iommu is used */
+#define INT_DSP_MMU_IRQ        (28 + NR_IRQS)
 
 #define PRCM_VDD1 1
 
diff --git a/drivers/staging/tidspbridge/rmgr/drv.c b/drivers/staging/tidspbridge/rmgr/drv.c
index 6795205b0155..db1da28cecba 100644
--- a/drivers/staging/tidspbridge/rmgr/drv.c
+++ b/drivers/staging/tidspbridge/rmgr/drv.c
@@ -667,10 +667,10 @@ int drv_request_bridge_res_dsp(void **phost_resources)
                                                          OMAP_DSP_MEM3_SIZE);
                 host_res->per_base = ioremap(OMAP_PER_CM_BASE,
                                                 OMAP_PER_CM_SIZE);
-                host_res->per_pm_base = (u32) ioremap(OMAP_PER_PRM_BASE,
-                                                         OMAP_PER_PRM_SIZE);
-                host_res->core_pm_base = (u32) ioremap(OMAP_CORE_PRM_BASE,
-                                                          OMAP_CORE_PRM_SIZE);
+                host_res->per_pm_base = ioremap(OMAP_PER_PRM_BASE,
+                                                OMAP_PER_PRM_SIZE);
+                host_res->core_pm_base = ioremap(OMAP_CORE_PRM_BASE,
+                                                        OMAP_CORE_PRM_SIZE);
                 host_res->dmmu_base = ioremap(OMAP_DMMU_BASE,
                                                  OMAP_DMMU_SIZE);
 
diff --git a/drivers/staging/tidspbridge/rmgr/node.c b/drivers/staging/tidspbridge/rmgr/node.c
index c2fc6137c770..294e9b40f516 100644
--- a/drivers/staging/tidspbridge/rmgr/node.c
+++ b/drivers/staging/tidspbridge/rmgr/node.c
@@ -304,8 +304,7 @@ int node_allocate(struct proc_object *hprocessor,
         u32 pul_value;
         u32 dynext_base;
         u32 off_set = 0;
-        u32 ul_stack_seg_addr, ul_stack_seg_val;
-        u32 ul_gpp_mem_base;
+        u32 ul_stack_seg_val;
         struct cfg_hostres *host_res;
         struct bridge_dev_context *pbridge_context;
         u32 mapped_addr = 0;
@@ -581,6 +580,9 @@ func_cont:
                 if (strcmp((char *)
                            pnode->dcd_props.obj_data.node_obj.ndb_props.
                            stack_seg_name, STACKSEGLABEL) == 0) {
+                        void __iomem *stack_seg;
+                        u32 stack_seg_pa;
+
                         status =
                             hnode_mgr->nldr_fxns.
                             get_fxn_addr(pnode->nldr_node_obj, "DYNEXT_BEG",
@@ -608,14 +610,21 @@ func_cont:
                                 goto func_end;
                         }
 
-                        ul_gpp_mem_base = (u32) host_res->mem_base[1];
                         off_set = pul_value - dynext_base;
-                        ul_stack_seg_addr = ul_gpp_mem_base + off_set;
-                        ul_stack_seg_val = readl(ul_stack_seg_addr);
+                        stack_seg_pa = host_res->mem_phys[1] + off_set;
+                        stack_seg = ioremap(stack_seg_pa, SZ_32);
+                        if (!stack_seg) {
+                                status = -ENOMEM;
+                                goto func_end;
+                        }
+
+                        ul_stack_seg_val = readl(stack_seg);
+
+                        iounmap(stack_seg);
 
                         dev_dbg(bridge, "%s: StackSegVal = 0x%x, StackSegAddr ="
                                 " 0x%x\n", __func__, ul_stack_seg_val,
-                                ul_stack_seg_addr);
+                                host_res->mem_base[1] + off_set);
 
                         pnode->create_args.asa.task_arg_obj.stack_seg =
                             ul_stack_seg_val;
diff --git a/drivers/staging/zram/zram_drv.c b/drivers/staging/zram/zram_drv.c
index 653b074035f7..6edefde23722 100644
--- a/drivers/staging/zram/zram_drv.c
+++ b/drivers/staging/zram/zram_drv.c
@@ -223,8 +223,13 @@ static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
         cmem = zs_map_object(zram->mem_pool, zram->table[index].handle,
                                 ZS_MM_RO);
 
-        ret = lzo1x_decompress_safe(cmem, zram->table[index].size,
+        if (zram->table[index].size == PAGE_SIZE) {
+                memcpy(uncmem, cmem, PAGE_SIZE);
+                ret = LZO_E_OK;
+        } else {
+                ret = lzo1x_decompress_safe(cmem, zram->table[index].size,
                                     uncmem, &clen);
+        }
 
         if (is_partial_io(bvec)) {
                 memcpy(user_mem + bvec->bv_offset, uncmem + offset,
@@ -342,8 +347,11 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
                 goto out;
         }
 
-        if (unlikely(clen > max_zpage_size))
+        if (unlikely(clen > max_zpage_size)) {
                 zram_stat_inc(&zram->stats.bad_compress);
+                src = uncmem;
+                clen = PAGE_SIZE;
+        }
 
         handle = zs_malloc(zram->mem_pool, clen);
         if (!handle) {
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 97c0f78c3c9c..035c2c762537 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -427,7 +427,7 @@ int iscsit_reset_np_thread(
         return 0;
 }
 
-int iscsit_del_np_comm(struct iscsi_np *np)
+static int iscsit_del_np_comm(struct iscsi_np *np)
 {
         if (np->np_socket)
                 sock_release(np->np_socket);
@@ -785,10 +785,6 @@ static int iscsit_handle_scsi_cmd(
 
         hdr                     = (struct iscsi_scsi_req *) buf;
         payload_length          = ntoh24(hdr->dlength);
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->data_length        = be32_to_cpu(hdr->data_length);
-        hdr->cmdsn              = be32_to_cpu(hdr->cmdsn);
-        hdr->exp_statsn         = be32_to_cpu(hdr->exp_statsn);
 
         /* FIXME; Add checks for AdditionalHeaderSegment */
 
@@ -852,7 +848,7 @@ done:
                                 buf, conn);
         }
 
-        if ((hdr->data_length == payload_length) &&
+        if ((be32_to_cpu(hdr->data_length )== payload_length) &&
             (!(hdr->flags & ISCSI_FLAG_CMD_FINAL))) {
                 pr_err("Expected Data Transfer Length and Length of"
                         " Immediate Data are the same, but ISCSI_FLAG_CMD_FINAL"
@@ -861,7 +857,7 @@ done:
                                 buf, conn);
         }
 
-        if (payload_length > hdr->data_length) {
+        if (payload_length > be32_to_cpu(hdr->data_length)) {
                 pr_err("DataSegmentLength: %u is greater than"
                         " EDTL: %u, protocol error.\n", payload_length,
                                 hdr->data_length);
@@ -869,10 +865,10 @@ done:
                                 buf, conn);
         }
 
-        if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+        if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
                 pr_err("DataSegmentLength: %u is greater than"
-                        " MaxRecvDataSegmentLength: %u, protocol error.\n",
-                        payload_length, conn->conn_ops->MaxRecvDataSegmentLength);
+                        " MaxXmitDataSegmentLength: %u, protocol error.\n",
+                        payload_length, conn->conn_ops->MaxXmitDataSegmentLength);
                 return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
                                 buf, conn);
         }
@@ -932,8 +928,8 @@ done:
                 spin_unlock_bh(&conn->sess->ttt_lock);
         } else if (hdr->flags & ISCSI_FLAG_CMD_WRITE)
                 cmd->targ_xfer_tag = 0xFFFFFFFF;
-        cmd->cmd_sn             = hdr->cmdsn;
-        cmd->exp_stat_sn        = hdr->exp_statsn;
+        cmd->cmd_sn             = be32_to_cpu(hdr->cmdsn);
+        cmd->exp_stat_sn        = be32_to_cpu(hdr->exp_statsn);
         cmd->first_burst_len    = payload_length;
 
         if (cmd->data_direction == DMA_FROM_DEVICE) {
@@ -952,8 +948,9 @@ done:
          * Initialize struct se_cmd descriptor from target_core_mod infrastructure
          */
         transport_init_se_cmd(&cmd->se_cmd, &lio_target_fabric_configfs->tf_ops,
-                        conn->sess->se_sess, hdr->data_length, cmd->data_direction,
-                        sam_task_attr, &cmd->sense_buffer[0]);
+                        conn->sess->se_sess, be32_to_cpu(hdr->data_length),
+                        cmd->data_direction, sam_task_attr,
+                        cmd->sense_buffer + 2);
 
         pr_debug("Got SCSI Command, ITT: 0x%08x, CmdSN: 0x%08x,"
                 " ExpXferLen: %u, Length: %u, CID: %hu\n", hdr->itt,
@@ -1028,7 +1025,7 @@ attach_cmd:
                                 1, 0, buf, cmd);
         }
 
-        iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+        iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
         /*
          * If no Immediate Data is attached, it's OK to return now.
@@ -1194,11 +1191,6 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 
         hdr                     = (struct iscsi_data *) buf;
         payload_length          = ntoh24(hdr->dlength);
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->ttt                = be32_to_cpu(hdr->ttt);
-        hdr->exp_statsn         = be32_to_cpu(hdr->exp_statsn);
-        hdr->datasn             = be32_to_cpu(hdr->datasn);
-        hdr->offset             = be32_to_cpu(hdr->offset);
 
         if (!payload_length) {
                 pr_err("DataOUT payload is ZERO, protocol error.\n");
@@ -1216,10 +1208,10 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
         }
         spin_unlock_bh(&conn->sess->session_stats_lock);
 
-        if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+        if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
                 pr_err("DataSegmentLength: %u is greater than"
-                        " MaxRecvDataSegmentLength: %u\n", payload_length,
-                        conn->conn_ops->MaxRecvDataSegmentLength);
+                        " MaxXmitDataSegmentLength: %u\n", payload_length,
+                        conn->conn_ops->MaxXmitDataSegmentLength);
                 return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
                                         buf, conn);
         }
@@ -1250,7 +1242,7 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
         se_cmd = &cmd->se_cmd;
         iscsit_mod_dataout_timer(cmd);
 
-        if ((hdr->offset + payload_length) > cmd->se_cmd.data_length) {
+        if ((be32_to_cpu(hdr->offset) + payload_length) > cmd->se_cmd.data_length) {
                 pr_err("DataOut Offset: %u, Length %u greater than"
                         " iSCSI Command EDTL %u, protocol error.\n",
                         hdr->offset, payload_length, cmd->se_cmd.data_length);
@@ -1333,7 +1325,8 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
         rx_size += payload_length;
         iov = &cmd->iov_data[0];
 
-        iov_ret = iscsit_map_iovec(cmd, iov, hdr->offset, payload_length);
+        iov_ret = iscsit_map_iovec(cmd, iov, be32_to_cpu(hdr->offset),
+                                   payload_length);
         if (iov_ret < 0)
                 return -1;
 
@@ -1364,7 +1357,8 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
                 u32 data_crc;
 
                 data_crc = iscsit_do_crypto_hash_sg(&conn->conn_rx_hash, cmd,
-                                                    hdr->offset, payload_length, padding,
+                                                    be32_to_cpu(hdr->offset),
+                                                    payload_length, padding,
                                                     cmd->pad_bytes);
 
                 if (checksum != data_crc) {
@@ -1425,30 +1419,26 @@ static int iscsit_handle_nop_out(
 
         hdr                     = (struct iscsi_nopout *) buf;
         payload_length          = ntoh24(hdr->dlength);
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->ttt                = be32_to_cpu(hdr->ttt);
-        hdr->cmdsn              = be32_to_cpu(hdr->cmdsn);
-        hdr->exp_statsn         = be32_to_cpu(hdr->exp_statsn);
 
-        if ((hdr->itt == 0xFFFFFFFF) && !(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
+        if (hdr->itt == RESERVED_ITT && !(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
                 pr_err("NOPOUT ITT is reserved, but Immediate Bit is"
                         " not set, protocol error.\n");
                 return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
                                         buf, conn);
         }
 
-        if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+        if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
                 pr_err("NOPOUT Ping Data DataSegmentLength: %u is"
-                        " greater than MaxRecvDataSegmentLength: %u, protocol"
+                        " greater than MaxXmitDataSegmentLength: %u, protocol"
                         " error.\n", payload_length,
-                        conn->conn_ops->MaxRecvDataSegmentLength);
+                        conn->conn_ops->MaxXmitDataSegmentLength);
                 return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
                                         buf, conn);
         }
 
         pr_debug("Got NOPOUT Ping %s ITT: 0x%08x, TTT: 0x%09x,"
                 " CmdSN: 0x%08x, ExpStatSN: 0x%08x, Length: %u\n",
-                (hdr->itt == 0xFFFFFFFF) ? "Response" : "Request",
+                hdr->itt == RESERVED_ITT ? "Response" : "Request",
                 hdr->itt, hdr->ttt, hdr->cmdsn, hdr->exp_statsn,
                 payload_length);
         /*
@@ -1458,7 +1448,7 @@ static int iscsit_handle_nop_out(
          * Either way, make sure we allocate an struct iscsi_cmd, as both
          * can contain ping data.
          */
-        if (hdr->ttt == 0xFFFFFFFF) {
+        if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
                 cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
                 if (!cmd)
                         return iscsit_add_reject(
@@ -1471,12 +1461,12 @@ static int iscsit_handle_nop_out(
                                                 1 : 0);
                 conn->sess->init_task_tag = cmd->init_task_tag = hdr->itt;
                 cmd->targ_xfer_tag      = 0xFFFFFFFF;
-                cmd->cmd_sn             = hdr->cmdsn;
-                cmd->exp_stat_sn        = hdr->exp_statsn;
+                cmd->cmd_sn             = be32_to_cpu(hdr->cmdsn);
+                cmd->exp_stat_sn        = be32_to_cpu(hdr->exp_statsn);
                 cmd->data_direction     = DMA_NONE;
         }
 
-        if (payload_length && (hdr->ttt == 0xFFFFFFFF)) {
+        if (payload_length && hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
                 rx_size = payload_length;
                 ping_data = kzalloc(payload_length + 1, GFP_KERNEL);
                 if (!ping_data) {
@@ -1556,7 +1546,7 @@ static int iscsit_handle_nop_out(
                 pr_debug("Ping Data: \"%s\"\n", ping_data);
         }
 
-        if (hdr->itt != 0xFFFFFFFF) {
+        if (hdr->itt != RESERVED_ITT) {
                 if (!cmd) {
                         pr_err("Checking CmdSN for NOPOUT,"
                                 " but cmd is NULL!\n");
@@ -1569,7 +1559,7 @@ static int iscsit_handle_nop_out(
                 list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
                 spin_unlock_bh(&conn->cmd_lock);
 
-                iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+                iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
                 if (hdr->opcode & ISCSI_OP_IMMEDIATE) {
                         iscsit_add_cmd_to_response_queue(cmd, conn,
@@ -1590,11 +1580,11 @@ static int iscsit_handle_nop_out(
                 return 0;
         }
 
-        if (hdr->ttt != 0xFFFFFFFF) {
+        if (hdr->ttt != cpu_to_be32(0xFFFFFFFF)) {
                 /*
                  * This was a response to a unsolicited NOPIN ping.
                  */
-                cmd = iscsit_find_cmd_from_ttt(conn, hdr->ttt);
+                cmd = iscsit_find_cmd_from_ttt(conn, be32_to_cpu(hdr->ttt));
                 if (!cmd)
                         return -1;
 
@@ -1639,12 +1629,6 @@ static int iscsit_handle_task_mgt_cmd(
         u8 function;
 
         hdr                     = (struct iscsi_tm *) buf;
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->rtt                = be32_to_cpu(hdr->rtt);
-        hdr->cmdsn              = be32_to_cpu(hdr->cmdsn);
-        hdr->exp_statsn         = be32_to_cpu(hdr->exp_statsn);
-        hdr->refcmdsn           = be32_to_cpu(hdr->refcmdsn);
-        hdr->exp_datasn         = be32_to_cpu(hdr->exp_datasn);
         hdr->flags &= ~ISCSI_FLAG_CMD_FINAL;
         function = hdr->flags;
 
@@ -1655,9 +1639,9 @@ static int iscsit_handle_task_mgt_cmd(
 
         if ((function != ISCSI_TM_FUNC_ABORT_TASK) &&
             ((function != ISCSI_TM_FUNC_TASK_REASSIGN) &&
-             (hdr->rtt != ISCSI_RESERVED_TAG))) {
+             hdr->rtt != RESERVED_ITT)) {
                 pr_err("RefTaskTag should be set to 0xFFFFFFFF.\n");
-                hdr->rtt = ISCSI_RESERVED_TAG;
+                hdr->rtt = RESERVED_ITT;
         }
 
         if ((function == ISCSI_TM_FUNC_TASK_REASSIGN) &&
@@ -1669,8 +1653,8 @@ static int iscsit_handle_task_mgt_cmd(
                                         buf, conn);
         }
         if ((function != ISCSI_TM_FUNC_ABORT_TASK) &&
-            (hdr->refcmdsn != ISCSI_RESERVED_TAG))
-                hdr->refcmdsn = ISCSI_RESERVED_TAG;
+            be32_to_cpu(hdr->refcmdsn) != ISCSI_RESERVED_TAG)
+                hdr->refcmdsn = cpu_to_be32(ISCSI_RESERVED_TAG);
 
         cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
         if (!cmd)
@@ -1700,7 +1684,7 @@ static int iscsit_handle_task_mgt_cmd(
                 transport_init_se_cmd(&cmd->se_cmd,
                                       &lio_target_fabric_configfs->tf_ops,
                                       conn->sess->se_sess, 0, DMA_NONE,
-                                      MSG_SIMPLE_TAG, &cmd->sense_buffer[0]);
+                                      MSG_SIMPLE_TAG, cmd->sense_buffer + 2);
 
                 switch (function) {
                 case ISCSI_TM_FUNC_ABORT_TASK:
@@ -1747,8 +1731,8 @@ static int iscsit_handle_task_mgt_cmd(
         cmd->immediate_cmd      = ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
         cmd->init_task_tag      = hdr->itt;
         cmd->targ_xfer_tag      = 0xFFFFFFFF;
-        cmd->cmd_sn             = hdr->cmdsn;
-        cmd->exp_stat_sn        = hdr->exp_statsn;
+        cmd->cmd_sn             = be32_to_cpu(hdr->cmdsn);
+        cmd->exp_stat_sn        = be32_to_cpu(hdr->exp_statsn);
         se_tmr                  = cmd->se_cmd.se_tmr_req;
         tmr_req                 = cmd->tmr_req;
         /*
@@ -1832,7 +1816,7 @@ attach:
                                         ISCSI_REASON_PROTOCOL_ERROR,
                                         1, 0, buf, cmd);
         }
-        iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+        iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
         if (out_of_order_cmdsn || !(hdr->opcode & ISCSI_OP_IMMEDIATE))
                 return 0;
@@ -1869,15 +1853,11 @@ static int iscsit_handle_text_cmd(
 
         hdr                     = (struct iscsi_text *) buf;
         payload_length          = ntoh24(hdr->dlength);
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->ttt                = be32_to_cpu(hdr->ttt);
-        hdr->cmdsn              = be32_to_cpu(hdr->cmdsn);
-        hdr->exp_statsn         = be32_to_cpu(hdr->exp_statsn);
 
-        if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+        if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
                 pr_err("Unable to accept text parameter length: %u"
-                        "greater than MaxRecvDataSegmentLength %u.\n",
-                       payload_length, conn->conn_ops->MaxRecvDataSegmentLength);
+                        "greater than MaxXmitDataSegmentLength %u.\n",
+                       payload_length, conn->conn_ops->MaxXmitDataSegmentLength);
                 return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
                                         buf, conn);
         }
@@ -1989,15 +1969,15 @@ static int iscsit_handle_text_cmd(
         cmd->immediate_cmd      = ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
         conn->sess->init_task_tag = cmd->init_task_tag  = hdr->itt;
         cmd->targ_xfer_tag      = 0xFFFFFFFF;
-        cmd->cmd_sn             = hdr->cmdsn;
-        cmd->exp_stat_sn        = hdr->exp_statsn;
+        cmd->cmd_sn             = be32_to_cpu(hdr->cmdsn);
+        cmd->exp_stat_sn        = be32_to_cpu(hdr->exp_statsn);
         cmd->data_direction     = DMA_NONE;
 
         spin_lock_bh(&conn->cmd_lock);
         list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
         spin_unlock_bh(&conn->cmd_lock);
 
-        iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+        iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
         if (!(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
                 cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
@@ -2131,10 +2111,6 @@ static int iscsit_handle_logout_cmd(
 
         hdr                     = (struct iscsi_logout *) buf;
         reason_code             = (hdr->flags & 0x7f);
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->cid                = be16_to_cpu(hdr->cid);
-        hdr->cmdsn              = be32_to_cpu(hdr->cmdsn);
-        hdr->exp_statsn = be32_to_cpu(hdr->exp_statsn);
 
         if (tiqn) {
                 spin_lock(&tiqn->logout_stats.lock);
@@ -2166,9 +2142,9 @@ static int iscsit_handle_logout_cmd(
         cmd->immediate_cmd      = ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
         conn->sess->init_task_tag = cmd->init_task_tag  = hdr->itt;
         cmd->targ_xfer_tag      = 0xFFFFFFFF;
-        cmd->cmd_sn             = hdr->cmdsn;
-        cmd->exp_stat_sn        = hdr->exp_statsn;
-        cmd->logout_cid         = hdr->cid;
+        cmd->cmd_sn             = be32_to_cpu(hdr->cmdsn);
+        cmd->exp_stat_sn        = be32_to_cpu(hdr->exp_statsn);
+        cmd->logout_cid         = be16_to_cpu(hdr->cid);
         cmd->logout_reason      = reason_code;
         cmd->data_direction     = DMA_NONE;
 
@@ -2178,7 +2154,7 @@ static int iscsit_handle_logout_cmd(
          */
         if ((reason_code == ISCSI_LOGOUT_REASON_CLOSE_SESSION) ||
            ((reason_code == ISCSI_LOGOUT_REASON_CLOSE_CONNECTION) &&
-            (hdr->cid == conn->cid)))
+            be16_to_cpu(hdr->cid) == conn->cid))
                 logout_remove = 1;
 
         spin_lock_bh(&conn->cmd_lock);
@@ -2186,7 +2162,7 @@ static int iscsit_handle_logout_cmd(
         spin_unlock_bh(&conn->cmd_lock);
 
         if (reason_code != ISCSI_LOGOUT_REASON_RECOVERY)
-                iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+                iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
         /*
          * Immediate commands are executed, well, immediately.
@@ -2219,11 +2195,6 @@ static int iscsit_handle_snack(
 
         hdr                     = (struct iscsi_snack *) buf;
         hdr->flags              &= ~ISCSI_FLAG_CMD_FINAL;
-        hdr->itt                = be32_to_cpu(hdr->itt);
-        hdr->ttt                = be32_to_cpu(hdr->ttt);
-        hdr->exp_statsn         = be32_to_cpu(hdr->exp_statsn);
-        hdr->begrun             = be32_to_cpu(hdr->begrun);
-        hdr->runlength          = be32_to_cpu(hdr->runlength);
 
         pr_debug("Got ISCSI_INIT_SNACK, ITT: 0x%08x, ExpStatSN:"
                 " 0x%08x, Type: 0x%02x, BegRun: 0x%08x, RunLength: 0x%08x,"
@@ -2243,13 +2214,18 @@ static int iscsit_handle_snack(
         switch (hdr->flags & ISCSI_FLAG_SNACK_TYPE_MASK) {
         case 0:
                 return iscsit_handle_recovery_datain_or_r2t(conn, buf,
-                        hdr->itt, hdr->ttt, hdr->begrun, hdr->runlength);
+                        hdr->itt,
+                        be32_to_cpu(hdr->ttt),
+                        be32_to_cpu(hdr->begrun),
+                        be32_to_cpu(hdr->runlength));
         case ISCSI_FLAG_SNACK_TYPE_STATUS:
-                return iscsit_handle_status_snack(conn, hdr->itt, hdr->ttt,
-                        hdr->begrun, hdr->runlength);
+                return iscsit_handle_status_snack(conn, hdr->itt,
+                        be32_to_cpu(hdr->ttt),
+                        be32_to_cpu(hdr->begrun), be32_to_cpu(hdr->runlength));
         case ISCSI_FLAG_SNACK_TYPE_DATA_ACK:
-                return iscsit_handle_data_ack(conn, hdr->ttt, hdr->begrun,
-                        hdr->runlength);
+                return iscsit_handle_data_ack(conn, be32_to_cpu(hdr->ttt),
+                        be32_to_cpu(hdr->begrun),
+                        be32_to_cpu(hdr->runlength));
         case ISCSI_FLAG_SNACK_TYPE_RDATA:
                 /* FIXME: Support R-Data SNACK */
                 pr_err("R-Data SNACK Not Supported.\n");
@@ -2414,7 +2390,7 @@ static int iscsit_send_conn_drop_async_message(
         hdr                     = (struct iscsi_async *) cmd->pdu;
         hdr->opcode             = ISCSI_OP_ASYNC_EVENT;
         hdr->flags              = ISCSI_FLAG_CMD_FINAL;
-        cmd->init_task_tag      = 0xFFFFFFFF;
+        cmd->init_task_tag      = RESERVED_ITT;
         cmd->targ_xfer_tag      = 0xFFFFFFFF;
         put_unaligned_be64(0xFFFFFFFFFFFFFFFFULL, &hdr->rsvd4[0]);
         cmd->stat_sn            = conn->stat_sn++;
@@ -2536,12 +2512,17 @@ static int iscsit_send_data_in(
         else
                 put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
 
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
-        hdr->ttt                = (hdr->flags & ISCSI_FLAG_DATA_ACK) ?
-                                   cpu_to_be32(cmd->targ_xfer_tag) :
-                                   0xFFFFFFFF;
-        hdr->statsn             = (set_statsn) ? cpu_to_be32(cmd->stat_sn) :
-                                                0xFFFFFFFF;
+        hdr->itt                = cmd->init_task_tag;
+
+        if (hdr->flags & ISCSI_FLAG_DATA_ACK)
+                hdr->ttt                = cpu_to_be32(cmd->targ_xfer_tag);
+        else
+                hdr->ttt                = cpu_to_be32(0xFFFFFFFF);
+        if (set_statsn)
+                hdr->statsn             = cpu_to_be32(cmd->stat_sn);
+        else
+                hdr->statsn             = cpu_to_be32(0xFFFFFFFF);
+
         hdr->exp_cmdsn          = cpu_to_be32(conn->sess->exp_cmd_sn);
         hdr->max_cmdsn          = cpu_to_be32(conn->sess->max_cmd_sn);
         hdr->datasn             = cpu_to_be32(datain.data_sn);
@@ -2708,7 +2689,7 @@ static int iscsit_send_logout_response(
         hdr->opcode             = ISCSI_OP_LOGOUT_RSP;
         hdr->flags              |= ISCSI_FLAG_CMD_FINAL;
         hdr->response           = cmd->logout_response;
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         cmd->stat_sn            = conn->stat_sn++;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
 
@@ -2759,7 +2740,7 @@ static int iscsit_send_unsolicited_nopin(
         memset(hdr, 0, ISCSI_HDR_LEN);
         hdr->opcode             = ISCSI_OP_NOOP_IN;
         hdr->flags              |= ISCSI_FLAG_CMD_FINAL;
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         hdr->ttt                = cpu_to_be32(cmd->targ_xfer_tag);
         cmd->stat_sn            = conn->stat_sn;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
@@ -2816,7 +2797,7 @@ static int iscsit_send_nopin_response(
         hdr->flags              |= ISCSI_FLAG_CMD_FINAL;
         hton24(hdr->dlength, cmd->buf_ptr_size);
         put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         hdr->ttt                = cpu_to_be32(cmd->targ_xfer_tag);
         cmd->stat_sn            = conn->stat_sn++;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
@@ -2906,7 +2887,7 @@ static int iscsit_send_r2t(
         hdr->flags              |= ISCSI_FLAG_CMD_FINAL;
         int_to_scsilun(cmd->se_cmd.orig_fe_lun,
                         (struct scsi_lun *)&hdr->lun);
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         spin_lock_bh(&conn->sess->ttt_lock);
         r2t->targ_xfer_tag      = conn->sess->targ_xfer_tag++;
         if (r2t->targ_xfer_tag == 0xFFFFFFFF)
@@ -3074,7 +3055,7 @@ static int iscsit_send_status(
         }
         hdr->response           = cmd->iscsi_response;
         hdr->cmd_status         = cmd->se_cmd.scsi_status;
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
 
         iscsit_increment_maxcmdsn(cmd, conn->sess);
@@ -3092,15 +3073,18 @@ static int iscsit_send_status(
         if (cmd->se_cmd.sense_buffer &&
            ((cmd->se_cmd.se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) ||
             (cmd->se_cmd.se_cmd_flags & SCF_EMULATED_TASK_SENSE))) {
+                put_unaligned_be16(cmd->se_cmd.scsi_sense_length, cmd->sense_buffer);
+                cmd->se_cmd.scsi_sense_length += sizeof (__be16);
+
                 padding         = -(cmd->se_cmd.scsi_sense_length) & 3;
-                hton24(hdr->dlength, cmd->se_cmd.scsi_sense_length);
-                iov[iov_count].iov_base = cmd->se_cmd.sense_buffer;
+                hton24(hdr->dlength, (u32)cmd->se_cmd.scsi_sense_length);
+                iov[iov_count].iov_base = cmd->sense_buffer;
                 iov[iov_count++].iov_len =
                                 (cmd->se_cmd.scsi_sense_length + padding);
                 tx_size += cmd->se_cmd.scsi_sense_length;
 
                 if (padding) {
-                        memset(cmd->se_cmd.sense_buffer +
+                        memset(cmd->sense_buffer +
                                 cmd->se_cmd.scsi_sense_length, 0, padding);
                         tx_size += padding;
                         pr_debug("Adding %u bytes of padding to"
@@ -3109,7 +3093,7 @@ static int iscsit_send_status(
 
                 if (conn->conn_ops->DataDigest) {
                         iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-                                cmd->se_cmd.sense_buffer,
+                                cmd->sense_buffer,
                                 (cmd->se_cmd.scsi_sense_length + padding),
                                 0, NULL, (u8 *)&cmd->data_crc);
 
@@ -3184,7 +3168,7 @@ static int iscsit_send_task_mgt_rsp(
         hdr->opcode             = ISCSI_OP_SCSI_TMFUNC_RSP;
         hdr->flags              = ISCSI_FLAG_CMD_FINAL;
         hdr->response           = iscsit_convert_tcm_tmr_rsp(se_tmr);
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         cmd->stat_sn            = conn->stat_sn++;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
 
@@ -3236,7 +3220,7 @@ static bool iscsit_check_inaddr_any(struct iscsi_np *np)
                 struct sockaddr_in * sock_in =
                         (struct sockaddr_in *)&np->np_sockaddr;
 
-                if (sock_in->sin_addr.s_addr == INADDR_ANY)
+                if (sock_in->sin_addr.s_addr == htonl(INADDR_ANY))
                         ret = true;
         }
 
@@ -3271,7 +3255,6 @@ static int iscsit_build_sendtargets_response(struct iscsi_cmd *cmd)
                 len += 1;
 
                 if ((len + payload_len) > buffer_len) {
-                        spin_unlock(&tiqn->tiqn_tpg_lock);
                         end_of_buf = 1;
                         goto eob;
                 }
@@ -3358,7 +3341,7 @@ static int iscsit_send_text_rsp(
         hdr->opcode             = ISCSI_OP_TEXT_RSP;
         hdr->flags              |= ISCSI_FLAG_CMD_FINAL;
         hton24(hdr->dlength, text_length);
-        hdr->itt                = cpu_to_be32(cmd->init_task_tag);
+        hdr->itt                = cmd->init_task_tag;
         hdr->ttt                = cpu_to_be32(cmd->targ_xfer_tag);
         cmd->stat_sn            = conn->stat_sn++;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
@@ -3424,6 +3407,7 @@ static int iscsit_send_reject(
         hdr->opcode             = ISCSI_OP_REJECT;
         hdr->flags              |= ISCSI_FLAG_CMD_FINAL;
         hton24(hdr->dlength, ISCSI_HDR_LEN);
+        hdr->ffffffff           = cpu_to_be32(0xffffffff);
         cmd->stat_sn            = conn->stat_sn++;
         hdr->statsn             = cpu_to_be32(cmd->stat_sn);
         hdr->exp_cmdsn  = cpu_to_be32(conn->sess->exp_cmd_sn);
@@ -3735,7 +3719,9 @@ restart:
                  */
                 iscsit_thread_check_cpumask(conn, current, 1);
 
-                schedule_timeout_interruptible(MAX_SCHEDULE_TIMEOUT);
+                wait_event_interruptible(conn->queues_wq,
+                                         !iscsit_conn_all_queues_empty(conn) ||
+                                         ts->status == ISCSI_THREAD_SET_RESET);
 
                 if ((ts->status == ISCSI_THREAD_SET_RESET) ||
                      signal_pending(current))
diff --git a/drivers/target/iscsi/iscsi_target.h b/drivers/target/iscsi/iscsi_target.h
index 12abb4c9e34e..f1e4f3155bac 100644
--- a/drivers/target/iscsi/iscsi_target.h
+++ b/drivers/target/iscsi/iscsi_target.h
@@ -38,4 +38,9 @@ extern struct kmem_cache *lio_cmd_cache;
 extern struct kmem_cache *lio_qr_cache;
 extern struct kmem_cache *lio_r2t_cache;
 
+extern struct idr sess_idr;
+extern struct mutex auth_id_lock;
+extern spinlock_t sess_idr_lock;
+
+
 #endif   /*** ISCSI_TARGET_H ***/
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index a7b25e783b58..ff6fd4fb624d 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -135,7 +135,7 @@ static struct configfs_attribute *lio_target_portal_attrs[] = {
 
 #define MAX_PORTAL_LEN          256
 
-struct se_tpg_np *lio_target_call_addnptotpg(
+static struct se_tpg_np *lio_target_call_addnptotpg(
         struct se_portal_group *se_tpg,
         struct config_group *group,
         const char *name)
@@ -1034,6 +1034,9 @@ TPG_PARAM_ATTR(ImmediateData, S_IRUGO | S_IWUSR);
 DEF_TPG_PARAM(MaxRecvDataSegmentLength);
 TPG_PARAM_ATTR(MaxRecvDataSegmentLength, S_IRUGO | S_IWUSR);
 
+DEF_TPG_PARAM(MaxXmitDataSegmentLength);
+TPG_PARAM_ATTR(MaxXmitDataSegmentLength, S_IRUGO | S_IWUSR);
+
 DEF_TPG_PARAM(MaxBurstLength);
 TPG_PARAM_ATTR(MaxBurstLength, S_IRUGO | S_IWUSR);
 
@@ -1079,6 +1082,7 @@ static struct configfs_attribute *lio_target_tpg_param_attrs[] = {
         &iscsi_tpg_param_InitialR2T.attr,
         &iscsi_tpg_param_ImmediateData.attr,
         &iscsi_tpg_param_MaxRecvDataSegmentLength.attr,
+        &iscsi_tpg_param_MaxXmitDataSegmentLength.attr,
         &iscsi_tpg_param_MaxBurstLength.attr,
         &iscsi_tpg_param_FirstBurstLength.attr,
         &iscsi_tpg_param_DefaultTime2Wait.attr,
@@ -1166,7 +1170,7 @@ static struct configfs_attribute *lio_target_tpg_attrs[] = {
 
 /* Start items for lio_target_tiqn_cit */
 
-struct se_portal_group *lio_target_tiqn_addtpg(
+static struct se_portal_group *lio_target_tiqn_addtpg(
         struct se_wwn *wwn,
         struct config_group *group,
         const char *name)
@@ -1216,7 +1220,7 @@ out:
         return NULL;
 }
 
-void lio_target_tiqn_deltpg(struct se_portal_group *se_tpg)
+static void lio_target_tiqn_deltpg(struct se_portal_group *se_tpg)
 {
         struct iscsi_portal_group *tpg;
         struct iscsi_tiqn *tiqn;
@@ -1248,7 +1252,7 @@ static struct configfs_attribute *lio_target_wwn_attrs[] = {
         NULL,
 };
 
-struct se_wwn *lio_target_call_coreaddtiqn(
+static struct se_wwn *lio_target_call_coreaddtiqn(
         struct target_fabric_configfs *tf,
         struct config_group *group,
         const char *name)
@@ -1296,7 +1300,7 @@ struct se_wwn *lio_target_call_coreaddtiqn(
         return &tiqn->tiqn_wwn;
 }
 
-void lio_target_call_coredeltiqn(
+static void lio_target_call_coredeltiqn(
         struct se_wwn *wwn)
 {
         struct iscsi_tiqn *tiqn = container_of(wwn, struct iscsi_tiqn, tiqn_wwn);
@@ -1471,7 +1475,8 @@ static u32 iscsi_get_task_tag(struct se_cmd *se_cmd)
 {
         struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
-        return cmd->init_task_tag;
+        /* only used for printks or comparism with ->ref_task_tag */
+        return (__force u32)cmd->init_task_tag;
 }
 
 static int iscsi_get_cmd_state(struct se_cmd *se_cmd)
@@ -1542,29 +1547,6 @@ static int lio_queue_status(struct se_cmd *se_cmd)
         return 0;
 }
 
-static u16 lio_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-        unsigned char *buffer = se_cmd->sense_buffer;
-        /*
-         * From RFC-3720 10.4.7.  Data Segment - Sense and Response Data Segment
-         * 16-bit SenseLength.
-         */
-        buffer[0] = ((sense_length >> 8) & 0xff);
-        buffer[1] = (sense_length & 0xff);
-        /*
-         * Return two byte offset into allocated sense_buffer.
-         */
-        return 2;
-}
-
-static u16 lio_get_fabric_sense_len(void)
-{
-        /*
-         * Return two byte offset into allocated sense_buffer.
-         */
-        return 2;
-}
-
 static int lio_queue_tm_rsp(struct se_cmd *se_cmd)
 {
         struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
@@ -1748,8 +1730,6 @@ int iscsi_target_register_configfs(void)
         fabric->tf_ops.queue_data_in = &lio_queue_data_in;
         fabric->tf_ops.queue_status = &lio_queue_status;
         fabric->tf_ops.queue_tm_rsp = &lio_queue_tm_rsp;
-        fabric->tf_ops.set_fabric_sense_len = &lio_set_fabric_sense_len;
-        fabric->tf_ops.get_fabric_sense_len = &lio_get_fabric_sense_len;
         /*
          * Setup function pointers for generic logic in target_core_fabric_configfs.c
          */
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 8a908b28d8b2..21048dbf7d13 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -25,10 +25,10 @@
 #define NA_DATAOUT_TIMEOUT_RETRIES      5
 #define NA_DATAOUT_TIMEOUT_RETRIES_MAX  15
 #define NA_DATAOUT_TIMEOUT_RETRIES_MIN  1
-#define NA_NOPIN_TIMEOUT                5
+#define NA_NOPIN_TIMEOUT                15
 #define NA_NOPIN_TIMEOUT_MAX            60
 #define NA_NOPIN_TIMEOUT_MIN            3
-#define NA_NOPIN_RESPONSE_TIMEOUT       5
+#define NA_NOPIN_RESPONSE_TIMEOUT       30
 #define NA_NOPIN_RESPONSE_TIMEOUT_MAX   60
 #define NA_NOPIN_RESPONSE_TIMEOUT_MIN   3
 #define NA_RANDOM_DATAIN_PDU_OFFSETS    0
@@ -239,6 +239,7 @@ struct iscsi_conn_ops {
         u8      HeaderDigest;                   /* [0,1] == [None,CRC32C] */
         u8      DataDigest;                     /* [0,1] == [None,CRC32C] */
         u32     MaxRecvDataSegmentLength;       /* [512..2**24-1] */
+        u32     MaxXmitDataSegmentLength;       /* [512..2**24-1] */
         u8      OFMarker;                       /* [0,1] == [No,Yes] */
         u8      IFMarker;                       /* [0,1] == [No,Yes] */
         u32     OFMarkInt;                      /* [1..65535] */
@@ -360,7 +361,7 @@ struct iscsi_cmd {
         /* Command flags */
         enum cmd_flags_table    cmd_flags;
         /* Initiator Task Tag assigned from Initiator */
-        u32                     init_task_tag;
+        itt_t                   init_task_tag;
         /* Target Transfer Tag assigned from Target */
         u32                     targ_xfer_tag;
         /* CmdSN assigned from Initiator */
@@ -478,7 +479,6 @@ struct iscsi_cmd {
 
 struct iscsi_tmr_req {
         bool                    task_reassign:1;
-        u32                     ref_cmd_sn;
         u32                     exp_data_sn;
         struct iscsi_cmd        *ref_cmd;
         struct iscsi_conn_recovery *conn_recovery;
@@ -486,6 +486,7 @@ struct iscsi_tmr_req {
 };
 
 struct iscsi_conn {
+        wait_queue_head_t       queues_wq;
         /* Authentication Successful for this connection */
         u8                      auth_complete;
         /* State connection is currently in */
@@ -505,7 +506,7 @@ struct iscsi_conn {
         u32                     auth_id;
         u32                     conn_flags;
         /* Used for iscsi_tx_login_rsp() */
-        u32                     login_itt;
+        itt_t                   login_itt;
         u32                     exp_statsn;
         /* Per connection status sequence number */
         u32                     stat_sn;
@@ -578,6 +579,7 @@ struct iscsi_conn_recovery {
         u16                     cid;
         u32                     cmd_count;
         u32                     maxrecvdatasegmentlength;
+        u32                     maxxmitdatasegmentlength;
         int                     ready_for_reallegiance;
         struct list_head        conn_recovery_cmd_list;
         spinlock_t              conn_recovery_cmd_lock;
@@ -597,7 +599,7 @@ struct iscsi_session {
         /* state session is currently in */
         u32                     session_state;
         /* session wide counter: initiator assigned task tag */
-        u32                     init_task_tag;
+        itt_t                   init_task_tag;
         /* session wide counter: target assigned task tag */
         u32                     targ_xfer_tag;
         u32                     cmdsn_window;
@@ -663,7 +665,7 @@ struct iscsi_login {
         u8 version_max;
         char isid[6];
         u32 cmd_sn;
-        u32 init_task_tag;
+        itt_t init_task_tag;
         u32 initial_exp_statsn;
         u32 rsp_length;
         u16 cid;
diff --git a/drivers/target/iscsi/iscsi_target_erl0.c b/drivers/target/iscsi/iscsi_target_erl0.c
index 1a02016ecdab..8aacf611b86d 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.c
+++ b/drivers/target/iscsi/iscsi_target_erl0.c
@@ -48,9 +48,9 @@ void iscsit_set_dataout_sequence_values(
         if (cmd->unsolicited_data) {
                 cmd->seq_start_offset = cmd->write_data_done;
                 cmd->seq_end_offset = (cmd->write_data_done +
-                        (cmd->se_cmd.data_length >
-                         conn->sess->sess_ops->FirstBurstLength) ?
-                        conn->sess->sess_ops->FirstBurstLength : cmd->se_cmd.data_length);
+                        ((cmd->se_cmd.data_length >
+                          conn->sess->sess_ops->FirstBurstLength) ?
+                         conn->sess->sess_ops->FirstBurstLength : cmd->se_cmd.data_length));
                 return;
         }
 
@@ -95,14 +95,15 @@ static int iscsit_dataout_within_command_recovery_check(
          */
         if (conn->sess->sess_ops->DataSequenceInOrder) {
                 if ((cmd->cmd_flags & ICF_WITHIN_COMMAND_RECOVERY) &&
-                    (cmd->write_data_done != hdr->offset))
+                    cmd->write_data_done != be32_to_cpu(hdr->offset))
                         goto dump;
 
                 cmd->cmd_flags &= ~ICF_WITHIN_COMMAND_RECOVERY;
         } else {
                 struct iscsi_seq *seq;
 
-                seq = iscsit_get_seq_holder(cmd, hdr->offset, payload_length);
+                seq = iscsit_get_seq_holder(cmd, be32_to_cpu(hdr->offset),
+                                            payload_length);
                 if (!seq)
                         return DATAOUT_CANNOT_RECOVER;
                 /*
@@ -111,15 +112,15 @@ static int iscsit_dataout_within_command_recovery_check(
                 cmd->seq_ptr = seq;
 
                 if (conn->sess->sess_ops->DataPDUInOrder) {
-                        if ((seq->status ==
-                             DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY) &&
-                           ((seq->offset != hdr->offset) ||
-                            (seq->data_sn != hdr->datasn)))
+                        if (seq->status ==
+                            DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY &&
+                           (seq->offset != be32_to_cpu(hdr->offset) ||
+                            seq->data_sn != be32_to_cpu(hdr->datasn)))
                                 goto dump;
                 } else {
-                        if ((seq->status ==
-                             DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY) &&
-                            (seq->data_sn != hdr->datasn))
+                        if (seq->status ==
+                             DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY &&
+                            seq->data_sn != be32_to_cpu(hdr->datasn))
                                 goto dump;
                 }
 
@@ -148,12 +149,12 @@ static int iscsit_dataout_check_unsolicited_sequence(
         u32 payload_length = ntoh24(hdr->dlength);
 
 
-        if ((hdr->offset < cmd->seq_start_offset) ||
-           ((hdr->offset + payload_length) > cmd->seq_end_offset)) {
+        if ((be32_to_cpu(hdr->offset) < cmd->seq_start_offset) ||
+           ((be32_to_cpu(hdr->offset) + payload_length) > cmd->seq_end_offset)) {
                 pr_err("Command ITT: 0x%08x with Offset: %u,"
                 " Length: %u outside of Unsolicited Sequence %u:%u while"
                 " DataSequenceInOrder=Yes.\n", cmd->init_task_tag,
-                hdr->offset, payload_length, cmd->seq_start_offset,
+                be32_to_cpu(hdr->offset), payload_length, cmd->seq_start_offset,
                         cmd->seq_end_offset);
                 return DATAOUT_CANNOT_RECOVER;
         }
@@ -236,12 +237,12 @@ static int iscsit_dataout_check_sequence(
                  * fullfilling an Recovery R2T, it's best to just dump the
                  * payload here, instead of erroring out.
                  */
-                if ((hdr->offset < cmd->seq_start_offset) ||
-                   ((hdr->offset + payload_length) > cmd->seq_end_offset)) {
+                if ((be32_to_cpu(hdr->offset) < cmd->seq_start_offset) ||
+                   ((be32_to_cpu(hdr->offset) + payload_length) > cmd->seq_end_offset)) {
                         pr_err("Command ITT: 0x%08x with Offset: %u,"
                         " Length: %u outside of Sequence %u:%u while"
                         " DataSequenceInOrder=Yes.\n", cmd->init_task_tag,
-                        hdr->offset, payload_length, cmd->seq_start_offset,
+                        be32_to_cpu(hdr->offset), payload_length, cmd->seq_start_offset,
                                 cmd->seq_end_offset);
 
                         if (iscsit_dump_data_payload(conn, payload_length, 1) < 0)
@@ -251,7 +252,8 @@ static int iscsit_dataout_check_sequence(
 
                 next_burst_len = (cmd->next_burst_len + payload_length);
         } else {
-                seq = iscsit_get_seq_holder(cmd, hdr->offset, payload_length);
+                seq = iscsit_get_seq_holder(cmd, be32_to_cpu(hdr->offset),
+                                            payload_length);
                 if (!seq)
                         return DATAOUT_CANNOT_RECOVER;
                 /*
@@ -366,16 +368,16 @@ static int iscsit_dataout_check_datasn(
                 data_sn = seq->data_sn;
         }
 
-        if (hdr->datasn > data_sn) {
+        if (be32_to_cpu(hdr->datasn) > data_sn) {
                 pr_err("Command ITT: 0x%08x, received DataSN: 0x%08x"
                         " higher than expected 0x%08x.\n", cmd->init_task_tag,
-                                hdr->datasn, data_sn);
+                                be32_to_cpu(hdr->datasn), data_sn);
                 recovery = 1;
                 goto recover;
-        } else if (hdr->datasn < data_sn) {
+        } else if (be32_to_cpu(hdr->datasn) < data_sn) {
                 pr_err("Command ITT: 0x%08x, received DataSN: 0x%08x"
                         " lower than expected 0x%08x, discarding payload.\n",
-                        cmd->init_task_tag, hdr->datasn, data_sn);
+                        cmd->init_task_tag, be32_to_cpu(hdr->datasn), data_sn);
                 dump = 1;
                 goto dump;
         }
@@ -415,26 +417,27 @@ static int iscsit_dataout_pre_datapduinorder_yes(
          * error has occured and fail the connection.
          */
         if (conn->sess->sess_ops->DataSequenceInOrder) {
-                if (hdr->offset != cmd->write_data_done) {
+                if (be32_to_cpu(hdr->offset) != cmd->write_data_done) {
                         pr_err("Command ITT: 0x%08x, received offset"
                         " %u different than expected %u.\n", cmd->init_task_tag,
-                                hdr->offset, cmd->write_data_done);
+                                be32_to_cpu(hdr->offset), cmd->write_data_done);
                         recovery = 1;
                         goto recover;
                 }
         } else {
                 struct iscsi_seq *seq = cmd->seq_ptr;
 
-                if (hdr->offset > seq->offset) {
+                if (be32_to_cpu(hdr->offset) > seq->offset) {
                         pr_err("Command ITT: 0x%08x, received offset"
                         " %u greater than expected %u.\n", cmd->init_task_tag,
-                                hdr->offset, seq->offset);
+                                be32_to_cpu(hdr->offset), seq->offset);
                         recovery = 1;
                         goto recover;
-                } else if (hdr->offset < seq->offset) {
+                } else if (be32_to_cpu(hdr->offset) < seq->offset) {
                         pr_err("Command ITT: 0x%08x, received offset"
                         " %u less than expected %u, discarding payload.\n",
-                                cmd->init_task_tag, hdr->offset, seq->offset);
+                                cmd->init_task_tag, be32_to_cpu(hdr->offset),
+                                seq->offset);
                         dump = 1;
                         goto dump;
                 }
@@ -453,7 +456,7 @@ dump:
                 return DATAOUT_CANNOT_RECOVER;
 
         return (recovery) ? iscsit_recover_dataout_sequence(cmd,
-                hdr->offset, payload_length) :
+                be32_to_cpu(hdr->offset), payload_length) :
                (dump) ? DATAOUT_WITHIN_COMMAND_RECOVERY : DATAOUT_NORMAL;
 }
 
@@ -465,7 +468,8 @@ static int iscsit_dataout_pre_datapduinorder_no(
         struct iscsi_data *hdr = (struct iscsi_data *) buf;
         u32 payload_length = ntoh24(hdr->dlength);
 
-        pdu = iscsit_get_pdu_holder(cmd, hdr->offset, payload_length);
+        pdu = iscsit_get_pdu_holder(cmd, be32_to_cpu(hdr->offset),
+                                    payload_length);
         if (!pdu)
                 return DATAOUT_CANNOT_RECOVER;
 
@@ -479,7 +483,7 @@ static int iscsit_dataout_pre_datapduinorder_no(
         case ISCSI_PDU_RECEIVED_OK:
                 pr_err("Command ITT: 0x%08x received already gotten"
                         " Offset: %u, Length: %u\n", cmd->init_task_tag,
-                                hdr->offset, payload_length);
+                                be32_to_cpu(hdr->offset), payload_length);
                 return iscsit_dump_data_payload(cmd->conn, payload_length, 1);
         default:
                 return DATAOUT_CANNOT_RECOVER;
@@ -553,7 +557,7 @@ static int iscsit_dataout_post_crc_passed(
         if (cmd->unsolicited_data) {
                 if ((cmd->first_burst_len + payload_length) ==
                      conn->sess->sess_ops->FirstBurstLength) {
-                        if (iscsit_dataout_update_r2t(cmd, hdr->offset,
+                        if (iscsit_dataout_update_r2t(cmd, be32_to_cpu(hdr->offset),
                                         payload_length) < 0)
                                 return DATAOUT_CANNOT_RECOVER;
                         send_r2t = 1;
@@ -561,7 +565,8 @@ static int iscsit_dataout_post_crc_passed(
 
                 if (!conn->sess->sess_ops->DataPDUInOrder) {
                         ret = iscsit_dataout_update_datapduinorder_no(cmd,
-                                hdr->datasn, (hdr->flags & ISCSI_FLAG_CMD_FINAL));
+                                be32_to_cpu(hdr->datasn),
+                                (hdr->flags & ISCSI_FLAG_CMD_FINAL));
                         if (ret == DATAOUT_CANNOT_RECOVER)
                                 return ret;
                 }
@@ -586,7 +591,8 @@ static int iscsit_dataout_post_crc_passed(
                 if (conn->sess->sess_ops->DataSequenceInOrder) {
                         if ((cmd->next_burst_len + payload_length) ==
                              conn->sess->sess_ops->MaxBurstLength) {
-                                if (iscsit_dataout_update_r2t(cmd, hdr->offset,
+                                if (iscsit_dataout_update_r2t(cmd,
+                                                be32_to_cpu(hdr->offset),
                                                 payload_length) < 0)
                                         return DATAOUT_CANNOT_RECOVER;
                                 send_r2t = 1;
@@ -594,7 +600,7 @@ static int iscsit_dataout_post_crc_passed(
 
                         if (!conn->sess->sess_ops->DataPDUInOrder) {
                                 ret = iscsit_dataout_update_datapduinorder_no(
-                                                cmd, hdr->datasn,
+                                                cmd, be32_to_cpu(hdr->datasn),
                                                 (hdr->flags & ISCSI_FLAG_CMD_FINAL));
                                 if (ret == DATAOUT_CANNOT_RECOVER)
                                         return ret;
@@ -610,7 +616,8 @@ static int iscsit_dataout_post_crc_passed(
 
                         if ((seq->next_burst_len + payload_length) ==
                              seq->xfer_len) {
-                                if (iscsit_dataout_update_r2t(cmd, hdr->offset,
+                                if (iscsit_dataout_update_r2t(cmd,
+                                                be32_to_cpu(hdr->offset),
                                                 payload_length) < 0)
                                         return DATAOUT_CANNOT_RECOVER;
                                 send_r2t = 1;
@@ -618,7 +625,7 @@ static int iscsit_dataout_post_crc_passed(
 
                         if (!conn->sess->sess_ops->DataPDUInOrder) {
                                 ret = iscsit_dataout_update_datapduinorder_no(
-                                                cmd, hdr->datasn,
+                                                cmd, be32_to_cpu(hdr->datasn),
                                                 (hdr->flags & ISCSI_FLAG_CMD_FINAL));
                                 if (ret == DATAOUT_CANNOT_RECOVER)
                                         return ret;
@@ -678,14 +685,15 @@ static int iscsit_dataout_post_crc_failed(
         }
 
 recover:
-        return iscsit_recover_dataout_sequence(cmd, hdr->offset, payload_length);
+        return iscsit_recover_dataout_sequence(cmd, be32_to_cpu(hdr->offset),
+                                                payload_length);
 }
 
 /*
  *      Called from iscsit_handle_data_out() before DataOUT Payload is received
  *      and CRC computed.
  */
-extern int iscsit_check_pre_dataout(
+int iscsit_check_pre_dataout(
         struct iscsi_cmd *cmd,
         unsigned char *buf)
 {
@@ -789,7 +797,7 @@ static void iscsit_handle_time2retain_timeout(unsigned long data)
         target_put_session(sess->se_sess);
 }
 
-extern void iscsit_start_time2retain_handler(struct iscsi_session *sess)
+void iscsit_start_time2retain_handler(struct iscsi_session *sess)
 {
         int tpg_active;
         /*
@@ -822,7 +830,7 @@ extern void iscsit_start_time2retain_handler(struct iscsi_session *sess)
 /*
  *      Called with spin_lock_bh(&struct se_portal_group->session_lock) held
  */
-extern int iscsit_stop_time2retain_timer(struct iscsi_session *sess)
+int iscsit_stop_time2retain_timer(struct iscsi_session *sess)
 {
         struct iscsi_portal_group *tpg = ISCSI_TPG_S(sess);
         struct se_portal_group *se_tpg = &tpg->tpg_se_tpg;
@@ -926,7 +934,7 @@ static void iscsit_handle_connection_cleanup(struct iscsi_conn *conn)
         }
 }
 
-extern void iscsit_take_action_for_connection_exit(struct iscsi_conn *conn)
+void iscsit_take_action_for_connection_exit(struct iscsi_conn *conn)
 {
         spin_lock_bh(&conn->state_lock);
         if (atomic_read(&conn->connection_exit)) {
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index 3df8a2cef86f..21f29d91a8cb 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -466,7 +466,7 @@ static int iscsit_handle_recovery_datain(
 int iscsit_handle_recovery_datain_or_r2t(
         struct iscsi_conn *conn,
         unsigned char *buf,
-        u32 init_task_tag,
+        itt_t init_task_tag,
         u32 targ_xfer_tag,
         u32 begrun,
         u32 runlength)
@@ -498,7 +498,7 @@ int iscsit_handle_recovery_datain_or_r2t(
 /* #warning FIXME: Status SNACK needs to be dependent on OPCODE!!! */
 int iscsit_handle_status_snack(
         struct iscsi_conn *conn,
-        u32 init_task_tag,
+        itt_t init_task_tag,
         u32 targ_xfer_tag,
         u32 begrun,
         u32 runlength)
diff --git a/drivers/target/iscsi/iscsi_target_erl1.h b/drivers/target/iscsi/iscsi_target_erl1.h
index 85e67e29de6b..2a3ebf118a34 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.h
+++ b/drivers/target/iscsi/iscsi_target_erl1.h
@@ -7,8 +7,8 @@ extern int iscsit_create_recovery_datain_values_datasequenceinorder_yes(
 extern int iscsit_create_recovery_datain_values_datasequenceinorder_no(
                         struct iscsi_cmd *, struct iscsi_datain_req *);
 extern int iscsit_handle_recovery_datain_or_r2t(struct iscsi_conn *, unsigned char *,
-                        u32, u32, u32, u32);
-extern int iscsit_handle_status_snack(struct iscsi_conn *, u32, u32,
+                        itt_t, u32, u32, u32);
+extern int iscsit_handle_status_snack(struct iscsi_conn *, itt_t, u32,
                         u32, u32);
 extern int iscsit_handle_data_ack(struct iscsi_conn *, u32, u32, u32);
 extern int iscsit_dataout_datapduinorder_no_fbit(struct iscsi_cmd *, struct iscsi_pdu *);
diff --git a/drivers/target/iscsi/iscsi_target_erl2.c b/drivers/target/iscsi/iscsi_target_erl2.c
index 65aac14fd831..17d8c20094fd 100644
--- a/drivers/target/iscsi/iscsi_target_erl2.c
+++ b/drivers/target/iscsi/iscsi_target_erl2.c
@@ -36,7 +36,7 @@
  */
 void iscsit_create_conn_recovery_datain_values(
         struct iscsi_cmd *cmd,
-        u32 exp_data_sn)
+        __be32 exp_data_sn)
 {
         u32 data_sn = 0;
         struct iscsi_conn *conn = cmd->conn;
@@ -44,7 +44,7 @@ void iscsit_create_conn_recovery_datain_values(
         cmd->next_burst_len = 0;
         cmd->read_data_done = 0;
 
-        while (exp_data_sn > data_sn) {
+        while (be32_to_cpu(exp_data_sn) > data_sn) {
                 if ((cmd->next_burst_len +
                      conn->conn_ops->MaxRecvDataSegmentLength) <
                      conn->sess->sess_ops->MaxBurstLength) {
@@ -193,15 +193,13 @@ int iscsit_remove_active_connection_recovery_entry(
         return 0;
 }
 
-int iscsit_remove_inactive_connection_recovery_entry(
+static void iscsit_remove_inactive_connection_recovery_entry(
         struct iscsi_conn_recovery *cr,
         struct iscsi_session *sess)
 {
         spin_lock(&sess->cr_i_lock);
         list_del(&cr->cr_list);
         spin_unlock(&sess->cr_i_lock);
-
-        return 0;
 }
 
 /*
@@ -421,6 +419,7 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
         cr->cid = conn->cid;
         cr->cmd_count = cmd_count;
         cr->maxrecvdatasegmentlength = conn->conn_ops->MaxRecvDataSegmentLength;
+        cr->maxxmitdatasegmentlength = conn->conn_ops->MaxXmitDataSegmentLength;
         cr->sess = conn->sess;
 
         iscsit_attach_inactive_connection_recovery_entry(conn->sess, cr);
diff --git a/drivers/target/iscsi/iscsi_target_erl2.h b/drivers/target/iscsi/iscsi_target_erl2.h
index 22f8d24780a6..63f2501f3fe0 100644
--- a/drivers/target/iscsi/iscsi_target_erl2.h
+++ b/drivers/target/iscsi/iscsi_target_erl2.h
@@ -1,7 +1,7 @@
 #ifndef ISCSI_TARGET_ERL2_H
 #define ISCSI_TARGET_ERL2_H
 
-extern void iscsit_create_conn_recovery_datain_values(struct iscsi_cmd *, u32);
+extern void iscsit_create_conn_recovery_datain_values(struct iscsi_cmd *, __be32);
 extern void iscsit_create_conn_recovery_dataout_values(struct iscsi_cmd *);
 extern struct iscsi_conn_recovery *iscsit_get_inactive_connection_recovery_entry(
                         struct iscsi_session *, u16);
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 6aba4395e8d8..f8dbec05d5e5 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -39,12 +39,9 @@
 #include "iscsi_target.h"
 #include "iscsi_target_parameters.h"
 
-extern struct idr sess_idr;
-extern struct mutex auth_id_lock;
-extern spinlock_t sess_idr_lock;
-
 static int iscsi_login_init_conn(struct iscsi_conn *conn)
 {
+        init_waitqueue_head(&conn->queues_wq);
         INIT_LIST_HEAD(&conn->conn_list);
         INIT_LIST_HEAD(&conn->conn_cmd_list);
         INIT_LIST_HEAD(&conn->immed_queue_list);
@@ -196,10 +193,10 @@ int iscsi_check_for_session_reinstatement(struct iscsi_conn *conn)
 static void iscsi_login_set_conn_values(
         struct iscsi_session *sess,
         struct iscsi_conn *conn,
-        u16 cid)
+        __be16 cid)
 {
         conn->sess              = sess;
-        conn->cid               = cid;
+        conn->cid               = be16_to_cpu(cid);
         /*
          * Generate a random Status sequence number (statsn) for the new
          * iSCSI connection.
@@ -234,7 +231,7 @@ static int iscsi_login_zero_tsih_s1(
         iscsi_login_set_conn_values(sess, conn, pdu->cid);
         sess->init_task_tag     = pdu->itt;
         memcpy(&sess->isid, pdu->isid, 6);
-        sess->exp_cmd_sn        = pdu->cmdsn;
+        sess->exp_cmd_sn        = be32_to_cpu(pdu->cmdsn);
         INIT_LIST_HEAD(&sess->sess_conn_list);
         INIT_LIST_HEAD(&sess->sess_ooo_cmdsn_list);
         INIT_LIST_HEAD(&sess->cr_active_list);
@@ -275,7 +272,7 @@ static int iscsi_login_zero_tsih_s1(
          * The FFP CmdSN window values will be allocated from the TPG's
          * Initiator Node's ACL once the login has been successfully completed.
          */
-        sess->max_cmd_sn        = pdu->cmdsn;
+        sess->max_cmd_sn        = be32_to_cpu(pdu->cmdsn);
 
         sess->sess_ops = kzalloc(sizeof(struct iscsi_sess_ops), GFP_KERNEL);
         if (!sess->sess_ops) {
@@ -453,7 +450,7 @@ static int iscsi_login_non_zero_tsih_s2(
                    (sess_p->time2retain_timer_flags & ISCSI_TF_EXPIRED))
                         continue;
                 if (!memcmp(sess_p->isid, pdu->isid, 6) &&
-                     (sess_p->tsih == pdu->tsih)) {
+                     (sess_p->tsih == be16_to_cpu(pdu->tsih))) {
                         iscsit_inc_session_usage_count(sess_p);
                         iscsit_stop_time2retain_timer(sess_p);
                         sess = sess_p;
@@ -955,11 +952,7 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
         }
 
         pdu                     = (struct iscsi_login_req *) buffer;
-        pdu->cid                = be16_to_cpu(pdu->cid);
-        pdu->tsih               = be16_to_cpu(pdu->tsih);
-        pdu->itt                = be32_to_cpu(pdu->itt);
-        pdu->cmdsn              = be32_to_cpu(pdu->cmdsn);
-        pdu->exp_statsn         = be32_to_cpu(pdu->exp_statsn);
+
         /*
          * Used by iscsit_tx_login_rsp() for Login Resonses PDUs
          * when Status-Class != 0.
diff --git a/drivers/target/iscsi/iscsi_target_nego.c b/drivers/target/iscsi/iscsi_target_nego.c
index 2dba448cac19..e9053a04f24c 100644
--- a/drivers/target/iscsi/iscsi_target_nego.c
+++ b/drivers/target/iscsi/iscsi_target_nego.c
@@ -44,7 +44,7 @@ void convert_null_to_semi(char *buf, int len)
                         buf[i] = ';';
 }
 
-int strlen_semi(char *buf)
+static int strlen_semi(char *buf)
 {
         int i = 0;
 
@@ -339,14 +339,14 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
         hton24(login_rsp->dlength, login->rsp_length);
         memcpy(login_rsp->isid, login->isid, 6);
         login_rsp->tsih                 = cpu_to_be16(login->tsih);
-        login_rsp->itt                  = cpu_to_be32(login->init_task_tag);
+        login_rsp->itt                  = login->init_task_tag;
         login_rsp->statsn               = cpu_to_be32(conn->stat_sn++);
         login_rsp->exp_cmdsn            = cpu_to_be32(conn->sess->exp_cmd_sn);
         login_rsp->max_cmdsn            = cpu_to_be32(conn->sess->max_cmd_sn);
 
         pr_debug("Sending Login Response, Flags: 0x%02x, ITT: 0x%08x,"
                 " ExpCmdSN; 0x%08x, MaxCmdSN: 0x%08x, StatSN: 0x%08x, Length:"
-                " %u\n", login_rsp->flags, ntohl(login_rsp->itt),
+                " %u\n", login_rsp->flags, (__force u32)login_rsp->itt,
                 ntohl(login_rsp->exp_cmdsn), ntohl(login_rsp->max_cmdsn),
                 ntohl(login_rsp->statsn), login->rsp_length);
 
@@ -360,12 +360,9 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
                 return -1;
 
         login->rsp_length               = 0;
-        login_rsp->tsih                 = be16_to_cpu(login_rsp->tsih);
-        login_rsp->itt                  = be32_to_cpu(login_rsp->itt);
-        login_rsp->statsn               = be32_to_cpu(login_rsp->statsn);
         mutex_lock(&sess->cmdsn_mutex);
-        login_rsp->exp_cmdsn            = be32_to_cpu(sess->exp_cmd_sn);
-        login_rsp->max_cmdsn            = be32_to_cpu(sess->max_cmd_sn);
+        login_rsp->exp_cmdsn            = cpu_to_be32(sess->exp_cmd_sn);
+        login_rsp->max_cmdsn            = cpu_to_be32(sess->max_cmd_sn);
         mutex_unlock(&sess->cmdsn_mutex);
 
         return 0;
@@ -381,11 +378,6 @@ static int iscsi_target_do_rx_login_io(struct iscsi_conn *conn, struct iscsi_log
 
         login_req = (struct iscsi_login_req *) login->req;
         payload_length                  = ntoh24(login_req->dlength);
-        login_req->tsih                 = be16_to_cpu(login_req->tsih);
-        login_req->itt                  = be32_to_cpu(login_req->itt);
-        login_req->cid                  = be16_to_cpu(login_req->cid);
-        login_req->cmdsn                = be32_to_cpu(login_req->cmdsn);
-        login_req->exp_statsn           = be32_to_cpu(login_req->exp_statsn);
 
         pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
                 " CmdSN: 0x%08x, ExpStatSN: 0x%08x, CID: %hu, Length: %u\n",
@@ -550,7 +542,7 @@ static int iscsi_target_handle_csg_zero(
                         SENDER_INITIATOR|SENDER_RECEIVER,
                         login->req_buf,
                         payload_length,
-                        conn->param_list);
+                        conn);
         if (ret < 0)
                 return -1;
 
@@ -627,7 +619,7 @@ static int iscsi_target_handle_csg_one(struct iscsi_conn *conn, struct iscsi_log
                         SENDER_INITIATOR|SENDER_RECEIVER,
                         login->req_buf,
                         payload_length,
-                        conn->param_list);
+                        conn);
         if (ret < 0)
                 return -1;
 
@@ -762,11 +754,11 @@ static int iscsi_target_locate_portal(
         login->version_min      = login_req->min_version;
         login->version_max      = login_req->max_version;
         memcpy(login->isid, login_req->isid, 6);
-        login->cmd_sn           = login_req->cmdsn;
+        login->cmd_sn           = be32_to_cpu(login_req->cmdsn);
         login->init_task_tag    = login_req->itt;
-        login->initial_exp_statsn = login_req->exp_statsn;
-        login->cid              = login_req->cid;
-        login->tsih             = login_req->tsih;
+        login->initial_exp_statsn = be32_to_cpu(login_req->exp_statsn);
+        login->cid              = be16_to_cpu(login_req->cid);
+        login->tsih             = be16_to_cpu(login_req->tsih);
 
         if (iscsi_target_get_initial_payload(conn, login) < 0)
                 return -1;
@@ -1000,7 +992,6 @@ struct iscsi_login *iscsi_target_init_negotiation(
          *      Locates Target Portal from NP -> Target IQN
          */
         if (iscsi_target_locate_portal(np, conn, login) < 0) {
-                pr_err("iSCSI Login negotiation failed.\n");
                 goto out;
         }
 
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index 240f7aa76ed1..90b740048f26 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -334,6 +334,13 @@ int iscsi_create_default_params(struct iscsi_param_list **param_list_ptr)
         if (!param)
                 goto out;
 
+        param = iscsi_set_default_param(pl, MAXXMITDATASEGMENTLENGTH,
+                        INITIAL_MAXXMITDATASEGMENTLENGTH,
+                        PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
+                        TYPERANGE_512_TO_16777215, USE_ALL);
+        if (!param)
+                goto out;
+
         param = iscsi_set_default_param(pl, MAXRECVDATASEGMENTLENGTH,
                         INITIAL_MAXRECVDATASEGMENTLENGTH,
                         PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
@@ -467,6 +474,8 @@ int iscsi_set_keys_to_negotiate(
                         SET_PSTATE_NEGOTIATE(param);
                 } else if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
                         SET_PSTATE_NEGOTIATE(param);
+                } else if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
+                        continue;
                 } else if (!strcmp(param->name, MAXBURSTLENGTH)) {
                         SET_PSTATE_NEGOTIATE(param);
                 } else if (!strcmp(param->name, FIRSTBURSTLENGTH)) {
@@ -1056,7 +1065,8 @@ out:
         return proposer_values;
 }
 
-static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value)
+static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value,
+                                struct iscsi_conn *conn)
 {
         u8 acceptor_boolean_value = 0, proposer_boolean_value = 0;
         char *negoitated_value = NULL;
@@ -1131,8 +1141,35 @@ static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value)
                                 return -1;
                 }
 
-                if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH))
-                        SET_PSTATE_REPLY_OPTIONAL(param);
+                if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
+                        struct iscsi_param *param_mxdsl;
+                        unsigned long long tmp;
+                        int rc;
+
+                        rc = strict_strtoull(param->value, 0, &tmp);
+                        if (rc < 0)
+                                return -1;
+
+                        conn->conn_ops->MaxRecvDataSegmentLength = tmp;
+                        pr_debug("Saving op->MaxRecvDataSegmentLength from"
+                                " original initiator received value: %u\n",
+                                conn->conn_ops->MaxRecvDataSegmentLength);
+
+                        param_mxdsl = iscsi_find_param_from_key(
+                                                MAXXMITDATASEGMENTLENGTH,
+                                                conn->param_list);
+                        if (!param_mxdsl)
+                                return -1;
+
+                        rc = iscsi_update_param_value(param,
+                                                param_mxdsl->value);
+                        if (rc < 0)
+                                return -1;
+
+                        pr_debug("Updated %s to target MXDSL value: %s\n",
+                                        param->name, param->value);
+                }
+
         } else if (IS_TYPE_NUMBER_RANGE(param)) {
                 negoitated_value = iscsi_get_value_from_number_range(
                                         param, value);
@@ -1526,8 +1563,9 @@ int iscsi_decode_text_input(
         u8 sender,
         char *textbuf,
         u32 length,
-        struct iscsi_param_list *param_list)
+        struct iscsi_conn *conn)
 {
+        struct iscsi_param_list *param_list = conn->param_list;
         char *tmpbuf, *start = NULL, *end = NULL;
 
         tmpbuf = kzalloc(length + 1, GFP_KERNEL);
@@ -1585,7 +1623,7 @@ int iscsi_decode_text_input(
                         }
                         SET_PSTATE_RESPONSE_GOT(param);
                 } else {
-                        if (iscsi_check_acceptor_state(param, value) < 0) {
+                        if (iscsi_check_acceptor_state(param, value, conn) < 0) {
                                 kfree(tmpbuf);
                                 return -1;
                         }
@@ -1720,6 +1758,18 @@ void iscsi_set_connection_parameters(
         pr_debug("---------------------------------------------------"
                         "---------------\n");
         list_for_each_entry(param, &param_list->param_list, p_list) {
+                /*
+                 * Special case to set MAXXMITDATASEGMENTLENGTH from the
+                 * target requested MaxRecvDataSegmentLength, even though
+                 * this key is not sent over the wire.
+                 */
+                if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
+                        ops->MaxXmitDataSegmentLength =
+                                simple_strtoul(param->value, &tmpptr, 0);
+                        pr_debug("MaxXmitDataSegmentLength:     %s\n",
+                                param->value);
+                }
+
                 if (!IS_PSTATE_ACCEPTOR(param) && !IS_PSTATE_PROPOSER(param))
                         continue;
                 if (!strcmp(param->name, AUTHMETHOD)) {
@@ -1734,10 +1784,13 @@ void iscsi_set_connection_parameters(
                         pr_debug("DataDigest:                   %s\n",
                                 param->value);
                 } else if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
-                        ops->MaxRecvDataSegmentLength =
-                                simple_strtoul(param->value, &tmpptr, 0);
-                        pr_debug("MaxRecvDataSegmentLength:     %s\n",
-                                param->value);
+                        /*
+                         * At this point iscsi_check_acceptor_state() will have
+                         * set ops->MaxRecvDataSegmentLength from the original
+                         * initiator provided value.
+                         */
+                        pr_debug("MaxRecvDataSegmentLength:     %u\n",
+                                ops->MaxRecvDataSegmentLength);
                 } else if (!strcmp(param->name, OFMARKER)) {
                         ops->OFMarker = !strcmp(param->value, YES);
                         pr_debug("OFMarker:                     %s\n",
diff --git a/drivers/target/iscsi/iscsi_target_parameters.h b/drivers/target/iscsi/iscsi_target_parameters.h
index 6a37fd6f1285..1e1b7504a76b 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.h
+++ b/drivers/target/iscsi/iscsi_target_parameters.h
@@ -36,7 +36,7 @@ extern void iscsi_release_param_list(struct iscsi_param_list *);
 extern struct iscsi_param *iscsi_find_param_from_key(char *, struct iscsi_param_list *);
 extern int iscsi_extract_key_value(char *, char **, char **);
 extern int iscsi_update_param_value(struct iscsi_param *, char *);
-extern int iscsi_decode_text_input(u8, u8, char *, u32, struct iscsi_param_list *);
+extern int iscsi_decode_text_input(u8, u8, char *, u32, struct iscsi_conn *);
 extern int iscsi_encode_text_output(u8, u8, char *, u32 *,
                         struct iscsi_param_list *);
 extern int iscsi_check_negotiated_keys(struct iscsi_param_list *);
@@ -70,6 +70,7 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIALR2T                      "InitialR2T"
 #define IMMEDIATEDATA                   "ImmediateData"
 #define MAXRECVDATASEGMENTLENGTH        "MaxRecvDataSegmentLength"
+#define MAXXMITDATASEGMENTLENGTH        "MaxXmitDataSegmentLength"
 #define MAXBURSTLENGTH                  "MaxBurstLength"
 #define FIRSTBURSTLENGTH                "FirstBurstLength"
 #define DEFAULTTIME2WAIT                "DefaultTime2Wait"
@@ -113,6 +114,10 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIAL_INITIALR2T                      YES
 #define INITIAL_IMMEDIATEDATA                   YES
 #define INITIAL_MAXRECVDATASEGMENTLENGTH        "8192"
+/*
+ * Match outgoing MXDSL default to incoming Open-iSCSI default
+ */
+#define INITIAL_MAXXMITDATASEGMENTLENGTH        "262144"
 #define INITIAL_MAXBURSTLENGTH                  "262144"
 #define INITIAL_FIRSTBURSTLENGTH                "65536"
 #define INITIAL_DEFAULTTIME2WAIT                "2"
diff --git a/drivers/target/iscsi/iscsi_target_seq_pdu_list.c b/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
index 85a306e067ba..edb592a368ef 100644
--- a/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
+++ b/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
@@ -219,8 +219,14 @@ static void iscsit_determine_counts_for_list(
         int check_immediate = 0;
         u32 burstlength = 0, offset = 0;
         u32 unsolicited_data_length = 0;
+        u32 mdsl;
         struct iscsi_conn *conn = cmd->conn;
 
+        if (cmd->se_cmd.data_direction == DMA_TO_DEVICE)
+                mdsl = cmd->conn->conn_ops->MaxXmitDataSegmentLength;
+        else
+                mdsl = cmd->conn->conn_ops->MaxRecvDataSegmentLength;
+
         if ((bl->type == PDULIST_IMMEDIATE) ||
             (bl->type == PDULIST_IMMEDIATE_AND_UNSOLICITED))
                 check_immediate = 1;
@@ -243,14 +249,13 @@ static void iscsit_determine_counts_for_list(
                         continue;
                 }
                 if (unsolicited_data_length > 0) {
-                        if ((offset + conn->conn_ops->MaxRecvDataSegmentLength)
-                                        >= cmd->se_cmd.data_length) {
+                        if ((offset + mdsl) >= cmd->se_cmd.data_length) {
                                 unsolicited_data_length -=
                                         (cmd->se_cmd.data_length - offset);
                                 offset += (cmd->se_cmd.data_length - offset);
                                 continue;
                         }
-                        if ((offset + conn->conn_ops->MaxRecvDataSegmentLength)
+                        if ((offset + mdsl)
                                         >= conn->sess->sess_ops->FirstBurstLength) {
                                 unsolicited_data_length -=
                                         (conn->sess->sess_ops->FirstBurstLength -
@@ -262,17 +267,15 @@ static void iscsit_determine_counts_for_list(
                                 continue;
                         }
 
-                        offset += conn->conn_ops->MaxRecvDataSegmentLength;
-                        unsolicited_data_length -=
-                                conn->conn_ops->MaxRecvDataSegmentLength;
+                        offset += mdsl;
+                        unsolicited_data_length -= mdsl;
                         continue;
                 }
-                if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-                     cmd->se_cmd.data_length) {
+                if ((offset + mdsl) >= cmd->se_cmd.data_length) {
                         offset += (cmd->se_cmd.data_length - offset);
                         continue;
                 }
-                if ((burstlength + conn->conn_ops->MaxRecvDataSegmentLength) >=
+                if ((burstlength + mdsl) >=
                      conn->sess->sess_ops->MaxBurstLength) {
                         offset += (conn->sess->sess_ops->MaxBurstLength -
                                         burstlength);
@@ -281,8 +284,8 @@ static void iscsit_determine_counts_for_list(
                         continue;
                 }
 
-                burstlength += conn->conn_ops->MaxRecvDataSegmentLength;
-                offset += conn->conn_ops->MaxRecvDataSegmentLength;
+                burstlength += mdsl;
+                offset += mdsl;
         }
 }
 
@@ -296,12 +299,17 @@ static int iscsit_do_build_pdu_and_seq_lists(
         struct iscsi_build_list *bl)
 {
         int check_immediate = 0, datapduinorder, datasequenceinorder;
-        u32 burstlength = 0, offset = 0, i = 0;
+        u32 burstlength = 0, offset = 0, i = 0, mdsl;
         u32 pdu_count = 0, seq_no = 0, unsolicited_data_length = 0;
         struct iscsi_conn *conn = cmd->conn;
         struct iscsi_pdu *pdu = cmd->pdu_list;
         struct iscsi_seq *seq = cmd->seq_list;
 
+        if (cmd->se_cmd.data_direction == DMA_TO_DEVICE)
+                mdsl = cmd->conn->conn_ops->MaxXmitDataSegmentLength;
+        else
+                mdsl = cmd->conn->conn_ops->MaxRecvDataSegmentLength;
+
         datapduinorder = conn->sess->sess_ops->DataPDUInOrder;
         datasequenceinorder = conn->sess->sess_ops->DataSequenceInOrder;
 
@@ -348,9 +356,7 @@ static int iscsit_do_build_pdu_and_seq_lists(
                         continue;
                 }
                 if (unsolicited_data_length > 0) {
-                        if ((offset +
-                             conn->conn_ops->MaxRecvDataSegmentLength) >=
-                             cmd->se_cmd.data_length) {
+                        if ((offset + mdsl) >= cmd->se_cmd.data_length) {
                                 if (!datapduinorder) {
                                         pdu[i].type = PDUTYPE_UNSOLICITED;
                                         pdu[i].length =
@@ -367,8 +373,7 @@ static int iscsit_do_build_pdu_and_seq_lists(
                                 offset += (cmd->se_cmd.data_length - offset);
                                 continue;
                         }
-                        if ((offset +
-                             conn->conn_ops->MaxRecvDataSegmentLength) >=
+                        if ((offset + mdsl) >=
                                         conn->sess->sess_ops->FirstBurstLength) {
                                 if (!datapduinorder) {
                                         pdu[i].type = PDUTYPE_UNSOLICITED;
@@ -396,17 +401,14 @@ static int iscsit_do_build_pdu_and_seq_lists(
 
                         if (!datapduinorder) {
                                 pdu[i].type = PDUTYPE_UNSOLICITED;
-                                pdu[i++].length =
-                                     conn->conn_ops->MaxRecvDataSegmentLength;
+                                pdu[i++].length = mdsl;
                         }
-                        burstlength += conn->conn_ops->MaxRecvDataSegmentLength;
-                        offset += conn->conn_ops->MaxRecvDataSegmentLength;
-                        unsolicited_data_length -=
-                                conn->conn_ops->MaxRecvDataSegmentLength;
+                        burstlength += mdsl;
+                        offset += mdsl;
+                        unsolicited_data_length -= mdsl;
                         continue;
                 }
-                if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-                     cmd->se_cmd.data_length) {
+                if ((offset + mdsl) >= cmd->se_cmd.data_length) {
                         if (!datapduinorder) {
                                 pdu[i].type = PDUTYPE_NORMAL;
                                 pdu[i].length = (cmd->se_cmd.data_length - offset);
@@ -420,7 +422,7 @@ static int iscsit_do_build_pdu_and_seq_lists(
                         offset += (cmd->se_cmd.data_length - offset);
                         continue;
                 }
-                if ((burstlength + conn->conn_ops->MaxRecvDataSegmentLength) >=
+                if ((burstlength + mdsl) >=
                      conn->sess->sess_ops->MaxBurstLength) {
                         if (!datapduinorder) {
                                 pdu[i].type = PDUTYPE_NORMAL;
@@ -445,11 +447,10 @@ static int iscsit_do_build_pdu_and_seq_lists(
 
                 if (!datapduinorder) {
                         pdu[i].type = PDUTYPE_NORMAL;
-                        pdu[i++].length =
-                                conn->conn_ops->MaxRecvDataSegmentLength;
+                        pdu[i++].length = mdsl;
                 }
-                burstlength += conn->conn_ops->MaxRecvDataSegmentLength;
-                offset += conn->conn_ops->MaxRecvDataSegmentLength;
+                burstlength += mdsl;
+                offset += mdsl;
         }
 
         if (!datasequenceinorder) {
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index f62fe123d902..4a99820d063b 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -50,21 +50,20 @@ u8 iscsit_tmr_abort_task(
         if (!ref_cmd) {
                 pr_err("Unable to locate RefTaskTag: 0x%08x on CID:"
                         " %hu.\n", hdr->rtt, conn->cid);
-                return ((hdr->refcmdsn >= conn->sess->exp_cmd_sn) &&
-                        (hdr->refcmdsn <= conn->sess->max_cmd_sn)) ?
+                return (be32_to_cpu(hdr->refcmdsn) >= conn->sess->exp_cmd_sn &&
+                        be32_to_cpu(hdr->refcmdsn) <= conn->sess->max_cmd_sn) ?
                         ISCSI_TMF_RSP_COMPLETE : ISCSI_TMF_RSP_NO_TASK;
         }
-        if (ref_cmd->cmd_sn != hdr->refcmdsn) {
+        if (ref_cmd->cmd_sn != be32_to_cpu(hdr->refcmdsn)) {
                 pr_err("RefCmdSN 0x%08x does not equal"
                         " task's CmdSN 0x%08x. Rejecting ABORT_TASK.\n",
                         hdr->refcmdsn, ref_cmd->cmd_sn);
                 return ISCSI_TMF_RSP_REJECTED;
         }
 
-        se_tmr->ref_task_tag            = hdr->rtt;
+        se_tmr->ref_task_tag            = (__force u32)hdr->rtt;
         tmr_req->ref_cmd                = ref_cmd;
-        tmr_req->ref_cmd_sn             = hdr->refcmdsn;
-        tmr_req->exp_data_sn            = hdr->exp_datasn;
+        tmr_req->exp_data_sn            = be32_to_cpu(hdr->exp_datasn);
 
         return ISCSI_TMF_RSP_COMPLETE;
 }
@@ -146,7 +145,7 @@ u8 iscsit_tmr_task_reassign(
         }
         /*
          * Temporary check to prevent connection recovery for
-         * connections with a differing MaxRecvDataSegmentLength.
+         * connections with a differing Max*DataSegmentLength.
          */
         if (cr->maxrecvdatasegmentlength !=
             conn->conn_ops->MaxRecvDataSegmentLength) {
@@ -155,6 +154,13 @@ u8 iscsit_tmr_task_reassign(
                         " TMR TASK_REASSIGN.\n");
                 return ISCSI_TMF_RSP_REJECTED;
         }
+        if (cr->maxxmitdatasegmentlength !=
+            conn->conn_ops->MaxXmitDataSegmentLength) {
+                pr_err("Unable to perform connection recovery for"
+                        " differing MaxXmitDataSegmentLength, rejecting"
+                        " TMR TASK_REASSIGN.\n");
+                return ISCSI_TMF_RSP_REJECTED;
+        }
 
         ref_lun = scsilun_to_int(&hdr->lun);
         if (ref_lun != ref_cmd->se_cmd.orig_fe_lun) {
@@ -164,10 +170,9 @@ u8 iscsit_tmr_task_reassign(
                 return ISCSI_TMF_RSP_REJECTED;
         }
 
-        se_tmr->ref_task_tag            = hdr->rtt;
+        se_tmr->ref_task_tag            = (__force u32)hdr->rtt;
         tmr_req->ref_cmd                = ref_cmd;
-        tmr_req->ref_cmd_sn             = hdr->refcmdsn;
-        tmr_req->exp_data_sn            = hdr->exp_datasn;
+        tmr_req->exp_data_sn            = be32_to_cpu(hdr->exp_datasn);
         tmr_req->conn_recovery          = cr;
         tmr_req->task_reassign          = 1;
         /*
@@ -455,7 +460,7 @@ static int iscsit_task_reassign_complete(
  *      Right now the only one that its really needed for is
  *      connection recovery releated TASK_REASSIGN.
  */
-extern int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 {
         struct iscsi_tmr_req *tmr_req = cmd->tmr_req;
         struct se_tmr_req *se_tmr = cmd->se_cmd.se_tmr_req;
@@ -470,7 +475,7 @@ extern int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *con
 /*
  *      Nothing to do here, but leave it for good measure. :-)
  */
-int iscsit_task_reassign_prepare_read(
+static int iscsit_task_reassign_prepare_read(
         struct iscsi_tmr_req *tmr_req,
         struct iscsi_conn *conn)
 {
@@ -545,7 +550,7 @@ static void iscsit_task_reassign_prepare_unsolicited_dataout(
         }
 }
 
-int iscsit_task_reassign_prepare_write(
+static int iscsit_task_reassign_prepare_write(
         struct iscsi_tmr_req *tmr_req,
         struct iscsi_conn *conn)
 {
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index a38a3f8ab0d9..de9ea32b6104 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -677,6 +677,12 @@ int iscsit_ta_generate_node_acls(
         pr_debug("iSCSI_TPG[%hu] - Generate Initiator Portal Group ACLs: %s\n",
                 tpg->tpgt, (a->generate_node_acls) ? "Enabled" : "Disabled");
 
+        if (flag == 1 && a->cache_dynamic_acls == 0) {
+                pr_debug("Explicitly setting cache_dynamic_acls=1 when "
+                        "generate_node_acls=1\n");
+                a->cache_dynamic_acls = 1;
+        }
+
         return 0;
 }
 
@@ -716,6 +722,12 @@ int iscsit_ta_cache_dynamic_acls(
                 return -EINVAL;
         }
 
+        if (a->generate_node_acls == 1 && flag == 0) {
+                pr_debug("Skipping cache_dynamic_acls=0 when"
+                        " generate_node_acls=1\n");
+                return 0;
+        }
+
         a->cache_dynamic_acls = flag;
         pr_debug("iSCSI_TPG[%hu] - Cache Dynamic Initiator Portal Group"
                 " ACLs %s\n", tpg->tpgt, (a->cache_dynamic_acls) ?
diff --git a/drivers/target/iscsi/iscsi_target_tq.c b/drivers/target/iscsi/iscsi_target_tq.c
index 977e1cf90e83..9d881a000e42 100644
--- a/drivers/target/iscsi/iscsi_target_tq.c
+++ b/drivers/target/iscsi/iscsi_target_tq.c
@@ -40,7 +40,7 @@ static void iscsi_add_ts_to_active_list(struct iscsi_thread_set *ts)
         spin_unlock(&active_ts_lock);
 }
 
-extern void iscsi_add_ts_to_inactive_list(struct iscsi_thread_set *ts)
+static void iscsi_add_ts_to_inactive_list(struct iscsi_thread_set *ts)
 {
         spin_lock(&inactive_ts_lock);
         list_add_tail(&ts->ts_list, &inactive_ts_list);
@@ -76,7 +76,7 @@ static struct iscsi_thread_set *iscsi_get_ts_from_inactive_list(void)
         return ts;
 }
 
-extern int iscsi_allocate_thread_sets(u32 thread_pair_count)
+int iscsi_allocate_thread_sets(u32 thread_pair_count)
 {
         int allocated_thread_pair_count = 0, i, thread_id;
         struct iscsi_thread_set *ts = NULL;
@@ -140,7 +140,7 @@ extern int iscsi_allocate_thread_sets(u32 thread_pair_count)
         return allocated_thread_pair_count;
 }
 
-extern void iscsi_deallocate_thread_sets(void)
+void iscsi_deallocate_thread_sets(void)
 {
         u32 released_count = 0;
         struct iscsi_thread_set *ts = NULL;
diff --git a/drivers/target/iscsi/iscsi_target_tq.h b/drivers/target/iscsi/iscsi_target_tq.h
index 26e6a95ec203..547d11831282 100644
--- a/drivers/target/iscsi/iscsi_target_tq.h
+++ b/drivers/target/iscsi/iscsi_target_tq.h
@@ -5,7 +5,6 @@
  * Defines for thread sets.
  */
 extern int iscsi_thread_set_force_reinstatement(struct iscsi_conn *);
-extern void iscsi_add_ts_to_inactive_list(struct iscsi_thread_set *);
 extern int iscsi_allocate_thread_sets(u32);
 extern void iscsi_deallocate_thread_sets(void);
 extern void iscsi_activate_thread_set(struct iscsi_conn *, struct iscsi_thread_set *);
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index b42cdeb153df..1a91195ab619 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -274,14 +274,14 @@ static inline int iscsit_check_received_cmdsn(struct iscsi_session *sess, u32 cm
 int iscsit_sequence_cmd(
         struct iscsi_conn *conn,
         struct iscsi_cmd *cmd,
-        u32 cmdsn)
+        __be32 cmdsn)
 {
         int ret;
         int cmdsn_ret;
 
         mutex_lock(&conn->sess->cmdsn_mutex);
 
-        cmdsn_ret = iscsit_check_received_cmdsn(conn->sess, cmdsn);
+        cmdsn_ret = iscsit_check_received_cmdsn(conn->sess, be32_to_cpu(cmdsn));
         switch (cmdsn_ret) {
         case CMDSN_NORMAL_OPERATION:
                 ret = iscsit_execute_cmd(cmd, 0);
@@ -289,7 +289,7 @@ int iscsit_sequence_cmd(
                         iscsit_execute_ooo_cmdsns(conn->sess);
                 break;
         case CMDSN_HIGHER_THAN_EXP:
-                ret = iscsit_handle_ooo_cmdsn(conn->sess, cmd, cmdsn);
+                ret = iscsit_handle_ooo_cmdsn(conn->sess, cmd, be32_to_cpu(cmdsn));
                 break;
         case CMDSN_LOWER_THAN_EXP:
                 cmd->i_state = ISTATE_REMOVE;
@@ -351,7 +351,7 @@ int iscsit_check_unsolicited_dataout(struct iscsi_cmd *cmd, unsigned char *buf)
 
 struct iscsi_cmd *iscsit_find_cmd_from_itt(
         struct iscsi_conn *conn,
-        u32 init_task_tag)
+        itt_t init_task_tag)
 {
         struct iscsi_cmd *cmd;
 
@@ -371,7 +371,7 @@ struct iscsi_cmd *iscsit_find_cmd_from_itt(
 
 struct iscsi_cmd *iscsit_find_cmd_from_itt_or_dump(
         struct iscsi_conn *conn,
-        u32 init_task_tag,
+        itt_t init_task_tag,
         u32 length)
 {
         struct iscsi_cmd *cmd;
@@ -417,7 +417,7 @@ int iscsit_find_cmd_for_recovery(
         struct iscsi_session *sess,
         struct iscsi_cmd **cmd_ptr,
         struct iscsi_conn_recovery **cr_ptr,
-        u32 init_task_tag)
+        itt_t init_task_tag)
 {
         struct iscsi_cmd *cmd = NULL;
         struct iscsi_conn_recovery *cr;
@@ -488,7 +488,7 @@ void iscsit_add_cmd_to_immediate_queue(
         atomic_set(&conn->check_immediate_queue, 1);
         spin_unlock_bh(&conn->immed_queue_lock);
 
-        wake_up_process(conn->thread_set->tx_thread);
+        wake_up(&conn->queues_wq);
 }
 
 struct iscsi_queue_req *iscsit_get_cmd_from_immediate_queue(struct iscsi_conn *conn)
@@ -562,7 +562,7 @@ void iscsit_add_cmd_to_response_queue(
         atomic_inc(&cmd->response_queue_count);
         spin_unlock_bh(&conn->response_queue_lock);
 
-        wake_up_process(conn->thread_set->tx_thread);
+        wake_up(&conn->queues_wq);
 }
 
 struct iscsi_queue_req *iscsit_get_cmd_from_response_queue(struct iscsi_conn *conn)
@@ -616,6 +616,24 @@ static void iscsit_remove_cmd_from_response_queue(
         }
 }
 
+bool iscsit_conn_all_queues_empty(struct iscsi_conn *conn)
+{
+        bool empty;
+
+        spin_lock_bh(&conn->immed_queue_lock);
+        empty = list_empty(&conn->immed_queue_list);
+        spin_unlock_bh(&conn->immed_queue_lock);
+
+        if (!empty)
+                return empty;
+
+        spin_lock_bh(&conn->response_queue_lock);
+        empty = list_empty(&conn->response_queue_list);
+        spin_unlock_bh(&conn->response_queue_lock);
+
+        return empty;
+}
+
 void iscsit_free_queue_reqs_for_conn(struct iscsi_conn *conn)
 {
         struct iscsi_queue_req *qr, *qr_tmp;
@@ -855,7 +873,7 @@ static int iscsit_add_nopin(struct iscsi_conn *conn, int want_response)
         cmd->iscsi_opcode = ISCSI_OP_NOOP_IN;
         state = (want_response) ? ISTATE_SEND_NOPIN_WANT_RESPONSE :
                                 ISTATE_SEND_NOPIN_NO_RESPONSE;
-        cmd->init_task_tag = 0xFFFFFFFF;
+        cmd->init_task_tag = RESERVED_ITT;
         spin_lock_bh(&conn->sess->ttt_lock);
         cmd->targ_xfer_tag = (want_response) ? conn->sess->targ_xfer_tag++ :
                         0xFFFFFFFF;
@@ -1222,7 +1240,7 @@ int iscsit_tx_login_rsp(struct iscsi_conn *conn, u8 status_class, u8 status_deta
         hdr->opcode             = ISCSI_OP_LOGIN_RSP;
         hdr->status_class       = status_class;
         hdr->status_detail      = status_detail;
-        hdr->itt                = cpu_to_be32(conn->login_itt);
+        hdr->itt                = conn->login_itt;
 
         iov.iov_base            = &iscsi_hdr;
         iov.iov_len             = ISCSI_HDR_LEN;
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index e1c729b8a1c5..894d0f837924 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -12,19 +12,20 @@ extern struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *, gfp_t);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_datain(struct iscsi_cmd *, u32);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_r2t(struct iscsi_cmd *);
 extern struct iscsi_r2t *iscsit_get_holder_for_r2tsn(struct iscsi_cmd *, u32);
-int iscsit_sequence_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd, u32 cmdsn);
+int iscsit_sequence_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd, __be32 cmdsn);
 extern int iscsit_check_unsolicited_dataout(struct iscsi_cmd *, unsigned char *);
-extern struct iscsi_cmd *iscsit_find_cmd_from_itt(struct iscsi_conn *, u32);
+extern struct iscsi_cmd *iscsit_find_cmd_from_itt(struct iscsi_conn *, itt_t);
 extern struct iscsi_cmd *iscsit_find_cmd_from_itt_or_dump(struct iscsi_conn *,
-                        u32, u32);
+                        itt_t, u32);
 extern struct iscsi_cmd *iscsit_find_cmd_from_ttt(struct iscsi_conn *, u32);
 extern int iscsit_find_cmd_for_recovery(struct iscsi_session *, struct iscsi_cmd **,
-                        struct iscsi_conn_recovery **, u32);
+                        struct iscsi_conn_recovery **, itt_t);
 extern void iscsit_add_cmd_to_immediate_queue(struct iscsi_cmd *, struct iscsi_conn *, u8);
 extern struct iscsi_queue_req *iscsit_get_cmd_from_immediate_queue(struct iscsi_conn *);
 extern void iscsit_add_cmd_to_response_queue(struct iscsi_cmd *, struct iscsi_conn *, u8);
 extern struct iscsi_queue_req *iscsit_get_cmd_from_response_queue(struct iscsi_conn *);
 extern void iscsit_remove_cmd_from_tx_queues(struct iscsi_cmd *, struct iscsi_conn *);
+extern bool iscsit_conn_all_queues_empty(struct iscsi_conn *);
 extern void iscsit_free_queue_reqs_for_conn(struct iscsi_conn *);
 extern void iscsit_release_cmd(struct iscsi_cmd *);
 extern void iscsit_free_cmd(struct iscsi_cmd *);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 5491c632a15e..2d444b1ccd33 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -166,7 +166,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
         struct tcm_loop_tpg *tl_tpg;
         struct scatterlist *sgl_bidi = NULL;
         u32 sgl_bidi_count = 0;
-        int ret;
+        int rc;
 
         tl_hba = *(struct tcm_loop_hba **)shost_priv(sc->device->host);
         tl_tpg = &tl_hba->tl_hba_tpgs[sc->device->id];
@@ -187,12 +187,6 @@ static void tcm_loop_submission_work(struct work_struct *work)
                 set_host_byte(sc, DID_ERROR);
                 goto out_done;
         }
-
-        transport_init_se_cmd(se_cmd, tl_tpg->tl_se_tpg.se_tpg_tfo,
-                        tl_nexus->se_sess,
-                        scsi_bufflen(sc), sc->sc_data_direction,
-                        tcm_loop_sam_attr(sc), &tl_cmd->tl_sense_buf[0]);
-
         if (scsi_bidi_cmnd(sc)) {
                 struct scsi_data_buffer *sdb = scsi_in(sc);
 
@@ -201,56 +195,16 @@ static void tcm_loop_submission_work(struct work_struct *work)
                 se_cmd->se_cmd_flags |= SCF_BIDI;
 
         }
-
-        if (transport_lookup_cmd_lun(se_cmd, tl_cmd->sc->device->lun) < 0) {
-                kmem_cache_free(tcm_loop_cmd_cache, tl_cmd);
+        rc = target_submit_cmd_map_sgls(se_cmd, tl_nexus->se_sess, sc->cmnd,
+                        &tl_cmd->tl_sense_buf[0], tl_cmd->sc->device->lun,
+                        scsi_bufflen(sc), tcm_loop_sam_attr(sc),
+                        sc->sc_data_direction, 0,
+                        scsi_sglist(sc), scsi_sg_count(sc),
+                        sgl_bidi, sgl_bidi_count);
+        if (rc < 0) {
                 set_host_byte(sc, DID_NO_CONNECT);
                 goto out_done;
         }
-
-        /*
-         * Because some userspace code via scsi-generic do not memset their
-         * associated read buffers, go ahead and do that here for type
-         * non-data CDBs.  Also note that this is currently guaranteed to be a
-         * single SGL for this case by target core in
-         * target_setup_cmd_from_cdb() -> transport_generic_cmd_sequencer().
-         */
-        if (!(se_cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) &&
-            se_cmd->data_direction == DMA_FROM_DEVICE) {
-                struct scatterlist *sg = scsi_sglist(sc);
-                unsigned char *buf = kmap(sg_page(sg)) + sg->offset;
-
-                if (buf != NULL) {
-                        memset(buf, 0, sg->length);
-                        kunmap(sg_page(sg));
-                }
-        }
-
-        ret = target_setup_cmd_from_cdb(se_cmd, sc->cmnd);
-        if (ret == -ENOMEM) {
-                transport_send_check_condition_and_sense(se_cmd,
-                                TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
-                transport_generic_free_cmd(se_cmd, 0);
-                return;
-        } else if (ret < 0) {
-                if (se_cmd->se_cmd_flags & SCF_SCSI_RESERVATION_CONFLICT)
-                        tcm_loop_queue_status(se_cmd);
-                else
-                        transport_send_check_condition_and_sense(se_cmd,
-                                        se_cmd->scsi_sense_reason, 0);
-                transport_generic_free_cmd(se_cmd, 0);
-                return;
-        }
-
-        ret = transport_generic_map_mem_to_cmd(se_cmd, scsi_sglist(sc),
-                        scsi_sg_count(sc), sgl_bidi, sgl_bidi_count);
-        if (ret) {
-                transport_send_check_condition_and_sense(se_cmd,
-                                        se_cmd->scsi_sense_reason, 0);
-                transport_generic_free_cmd(se_cmd, 0);
-                return;
-        }
-        transport_handle_cdb_direct(se_cmd);
         return;
 
 out_done:
@@ -846,16 +800,6 @@ static int tcm_loop_queue_tm_rsp(struct se_cmd *se_cmd)
         return 0;
 }
 
-static u16 tcm_loop_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-        return 0;
-}
-
-static u16 tcm_loop_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
 static char *tcm_loop_dump_proto_id(struct tcm_loop_hba *tl_hba)
 {
         switch (tl_hba->tl_proto_id) {
@@ -1373,8 +1317,6 @@ static int tcm_loop_register_configfs(void)
         fabric->tf_ops.queue_data_in = &tcm_loop_queue_data_in;
         fabric->tf_ops.queue_status = &tcm_loop_queue_status;
         fabric->tf_ops.queue_tm_rsp = &tcm_loop_queue_tm_rsp;
-        fabric->tf_ops.set_fabric_sense_len = &tcm_loop_set_fabric_sense_len;
-        fabric->tf_ops.get_fabric_sense_len = &tcm_loop_get_fabric_sense_len;
 
         /*
          * Setup function pointers for generic logic in target_core_fabric_configfs.c
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
index 39ddba584b30..0d6d7c1f025e 100644
--- a/drivers/target/sbp/sbp_target.c
+++ b/drivers/target/sbp/sbp_target.c
@@ -660,8 +660,7 @@ static void session_reconnect_expired(struct sbp_session *sess)
         spin_lock_bh(&sess->lock);
         list_for_each_entry_safe(login, temp, &sess->login_list, link) {
                 login->sess = NULL;
-                list_del(&login->link);
-                list_add_tail(&login->link, &login_list);
+                list_move_tail(&login->link, &login_list);
         }
         spin_unlock_bh(&sess->lock);
 
@@ -1847,16 +1846,6 @@ static int sbp_queue_tm_rsp(struct se_cmd *se_cmd)
         return 0;
 }
 
-static u16 sbp_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-        return 0;
-}
-
-static u16 sbp_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
 static int sbp_check_stop_free(struct se_cmd *se_cmd)
 {
         struct sbp_target_request *req = container_of(se_cmd,
@@ -2068,7 +2057,7 @@ static int sbp_update_unit_directory(struct sbp_tport *tport)
         return ret;
 }
 
-static ssize_t sbp_parse_wwn(const char *name, u64 *wwn, int strict)
+static ssize_t sbp_parse_wwn(const char *name, u64 *wwn)
 {
         const char *cp;
         char c, nibble;
@@ -2088,7 +2077,7 @@ static ssize_t sbp_parse_wwn(const char *name, u64 *wwn, int strict)
                 err = 3;
                 if (isdigit(c))
                         nibble = c - '0';
-                else if (isxdigit(c) && (islower(c) || !strict))
+                else if (isxdigit(c))
                         nibble = tolower(c) - 'a' + 10;
                 else
                         goto fail;
@@ -2117,7 +2106,7 @@ static struct se_node_acl *sbp_make_nodeacl(
         u64 guid = 0;
         u32 nexus_depth = 1;
 
-        if (sbp_parse_wwn(name, &guid, 1) < 0)
+        if (sbp_parse_wwn(name, &guid) < 0)
                 return ERR_PTR(-EINVAL);
 
         se_nacl_new = sbp_alloc_fabric_acl(se_tpg);
@@ -2253,7 +2242,7 @@ static struct se_wwn *sbp_make_tport(
         struct sbp_tport *tport;
         u64 guid = 0;
 
-        if (sbp_parse_wwn(name, &guid, 1) < 0)
+        if (sbp_parse_wwn(name, &guid) < 0)
                 return ERR_PTR(-EINVAL);
 
         tport = kzalloc(sizeof(*tport), GFP_KERNEL);
@@ -2534,8 +2523,6 @@ static struct target_core_fabric_ops sbp_ops = {
         .queue_data_in                  = sbp_queue_data_in,
         .queue_status                   = sbp_queue_status,
         .queue_tm_rsp                   = sbp_queue_tm_rsp,
-        .get_fabric_sense_len           = sbp_get_fabric_sense_len,
-        .set_fabric_sense_len           = sbp_set_fabric_sense_len,
         .check_stop_free                = sbp_check_stop_free,
 
         .fabric_make_wwn                = sbp_make_tport,
@@ -2556,9 +2543,9 @@ static int sbp_register_configfs(void)
         int ret;
 
         fabric = target_fabric_configfs_init(THIS_MODULE, "sbp");
-        if (!fabric) {
+        if (IS_ERR(fabric)) {
                 pr_err("target_fabric_configfs_init() failed\n");
-                return -ENOMEM;
+                return PTR_ERR(fabric);
         }
 
         fabric->tf_ops = sbp_ops;
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index 41641ba54828..9a5f9a7aecd2 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -344,7 +344,7 @@ int target_emulate_set_target_port_groups(struct se_cmd *cmd)
                          */
                         rtpi = get_unaligned_be16(ptr + 2);
                         /*
-                         * Locate the matching relative target port identifer
+                         * Locate the matching relative target port identifier
                          * for the struct se_device storage object.
                          */
                         spin_lock(&dev->se_port_lock);
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index 801efa892046..c123327499a3 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -457,14 +457,6 @@ static int target_fabric_tf_ops_check(
                 pr_err("Missing tfo->queue_tm_rsp()\n");
                 return -EINVAL;
         }
-        if (!tfo->set_fabric_sense_len) {
-                pr_err("Missing tfo->set_fabric_sense_len()\n");
-                return -EINVAL;
-        }
-        if (!tfo->get_fabric_sense_len) {
-                pr_err("Missing tfo->get_fabric_sense_len()\n");
-                return -EINVAL;
-        }
         /*
          * We at least require tfo->fabric_make_wwn(), tfo->fabric_drop_wwn()
          * tfo->fabric_make_tpg() and tfo->fabric_drop_tpg() in
@@ -1208,7 +1200,7 @@ static ssize_t target_core_dev_pr_show_attr_res_pr_holder_tg_port(
                 " Target Node Endpoint: %s\n", tfo->get_fabric_name(),
                 tfo->tpg_get_wwn(se_tpg));
         len += sprintf(page+len, "SPC-3 Reservation: Relative Port"
-                " Identifer Tag: %hu %s Portal Group Tag: %hu"
+                " Identifier Tag: %hu %s Portal Group Tag: %hu"
                 " %s Logical Unit: %u\n", lun->lun_sep->sep_rtpi,
                 tfo->get_fabric_name(), tfo->tpg_get_tag(se_tpg),
                 tfo->get_fabric_name(), lun->unpacked_lun);
@@ -3132,6 +3124,7 @@ static int __init target_core_init_configfs(void)
                                 GFP_KERNEL);
         if (!target_cg->default_groups) {
                 pr_err("Unable to allocate target_cg->default_groups\n");
+                ret = -ENOMEM;
                 goto out_global;
         }
 
@@ -3147,6 +3140,7 @@ static int __init target_core_init_configfs(void)
                                 GFP_KERNEL);
         if (!hba_cg->default_groups) {
                 pr_err("Unable to allocate hba_cg->default_groups\n");
+                ret = -ENOMEM;
                 goto out_global;
         }
         config_group_init_type_name(&alua_group,
@@ -3162,6 +3156,7 @@ static int __init target_core_init_configfs(void)
                         GFP_KERNEL);
         if (!alua_cg->default_groups) {
                 pr_err("Unable to allocate alua_cg->default_groups\n");
+                ret = -ENOMEM;
                 goto out_global;
         }
 
@@ -3173,14 +3168,17 @@ static int __init target_core_init_configfs(void)
          * Add core/alua/lu_gps/default_lu_gp
          */
         lu_gp = core_alua_allocate_lu_gp("default_lu_gp", 1);
-        if (IS_ERR(lu_gp))
+        if (IS_ERR(lu_gp)) {
+                ret = -ENOMEM;
                 goto out_global;
+        }
 
         lu_gp_cg = &alua_lu_gps_group;
         lu_gp_cg->default_groups = kzalloc(sizeof(struct config_group) * 2,
                         GFP_KERNEL);
         if (!lu_gp_cg->default_groups) {
                 pr_err("Unable to allocate lu_gp_cg->default_groups\n");
+                ret = -ENOMEM;
                 goto out_global;
         }
 
@@ -3208,7 +3206,8 @@ static int __init target_core_init_configfs(void)
         if (ret < 0)
                 goto out;
 
-        if (core_dev_setup_virtual_lun0() < 0)
+        ret = core_dev_setup_virtual_lun0();
+        if (ret < 0)
                 goto out;
 
         return 0;
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 9fc9a6006ca0..9abef9f8eb76 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -531,7 +531,7 @@ static struct se_port *core_alloc_port(struct se_device *dev)
         }
 again:
         /*
-         * Allocate the next RELATIVE TARGET PORT IDENTIFER for this struct se_device
+         * Allocate the next RELATIVE TARGET PORT IDENTIFIER for this struct se_device
          * Here is the table from spc4r17 section 7.7.3.8.
          *
          *    Table 473 -- RELATIVE TARGET PORT IDENTIFIER field
@@ -548,7 +548,7 @@ again:
 
         list_for_each_entry(port_tmp, &dev->dev_sep_list, sep_list) {
                 /*
-                 * Make sure RELATIVE TARGET PORT IDENTIFER is unique
+                 * Make sure RELATIVE TARGET PORT IDENTIFIER is unique
                  * for 16-bit wrap..
                  */
                 if (port->sep_rtpi == port_tmp->sep_rtpi)
@@ -595,7 +595,7 @@ static void core_export_port(
         }
 
         dev->dev_port_count++;
-        port->sep_index = port->sep_rtpi; /* RELATIVE TARGET PORT IDENTIFER */
+        port->sep_index = port->sep_rtpi; /* RELATIVE TARGET PORT IDENTIFIER */
 }
 
 /*
@@ -850,20 +850,20 @@ int se_dev_check_shutdown(struct se_device *dev)
 
 static u32 se_dev_align_max_sectors(u32 max_sectors, u32 block_size)
 {
-        u32 tmp, aligned_max_sectors;
+        u32 aligned_max_sectors;
+        u32 alignment;
         /*
          * Limit max_sectors to a PAGE_SIZE aligned value for modern
          * transport_allocate_data_tasks() operation.
          */
-        tmp = rounddown((max_sectors * block_size), PAGE_SIZE);
-        aligned_max_sectors = (tmp / block_size);
-        if (max_sectors != aligned_max_sectors) {
-                printk(KERN_INFO "Rounding down aligned max_sectors from %u"
-                                " to %u\n", max_sectors, aligned_max_sectors);
-                return aligned_max_sectors;
-        }
+        alignment = max(1ul, PAGE_SIZE / block_size);
+        aligned_max_sectors = rounddown(max_sectors, alignment);
+
+        if (max_sectors != aligned_max_sectors)
+                pr_info("Rounding down aligned max_sectors from %u to %u\n",
+                        max_sectors, aligned_max_sectors);
 
-        return max_sectors;
+        return aligned_max_sectors;
 }
 
 void se_dev_set_default_attribs(
@@ -988,8 +988,9 @@ int se_dev_set_emulate_fua_write(struct se_device *dev, int flag)
                 return -EINVAL;
         }
 
-        if (flag && dev->transport->fua_write_emulated == 0) {
-                pr_err("fua_write_emulated not supported\n");
+        if (flag &&
+            dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
+                pr_err("emulate_fua_write not supported for pSCSI\n");
                 return -EINVAL;
         }
         dev->se_sub_dev->se_dev_attrib.emulate_fua_write = flag;
@@ -1019,8 +1020,9 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
                 pr_err("Illegal value %d\n", flag);
                 return -EINVAL;
         }
-        if (flag && dev->transport->write_cache_emulated == 0) {
-                pr_err("write_cache_emulated not supported\n");
+        if (flag &&
+            dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
+                pr_err("emulate_write_cache not supported for pSCSI\n");
                 return -EINVAL;
         }
         dev->se_sub_dev->se_dev_attrib.emulate_write_cache = flag;
diff --git a/drivers/target/target_core_fabric_configfs.c b/drivers/target/target_core_fabric_configfs.c
index ea479e54f5fd..bca737bb813d 100644
--- a/drivers/target/target_core_fabric_configfs.c
+++ b/drivers/target/target_core_fabric_configfs.c
@@ -22,7 +22,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/fs.h>
diff --git a/drivers/target/target_core_fabric_lib.c b/drivers/target/target_core_fabric_lib.c
index 283a36e464e6..e460d6233a0a 100644
--- a/drivers/target/target_core_fabric_lib.c
+++ b/drivers/target/target_core_fabric_lib.c
@@ -338,7 +338,7 @@ u32 iscsi_get_pr_transport_id_len(
          * 00b: iSCSI Initiator device TransportID format
          */
         if (pr_reg->isid_present_at_reg) {
-                len += 5; /* For ",i,0x" ASCII seperator */
+                len += 5; /* For ",i,0x" ASCII separator */
                 len += 7; /* For iSCSI Initiator Session ID + Null terminator */
                 *format_code = 1;
         } else
@@ -415,20 +415,20 @@ char *iscsi_parse_pr_out_transport_id(
                         *out_tid_len = (add_len + 4);
         }
         /*
-         * Check for ',i,0x' seperator between iSCSI Name and iSCSI Initiator
+         * Check for ',i,0x' separator between iSCSI Name and iSCSI Initiator
          * Session ID as defined in Table 390 - iSCSI initiator port TransportID
          * format.
          */
         if (format_code == 0x40) {
                 p = strstr(&buf[4], ",i,0x");
                 if (!p) {
-                        pr_err("Unable to locate \",i,0x\" seperator"
+                        pr_err("Unable to locate \",i,0x\" separator"
                                 " for Initiator port identifier: %s\n",
                                 &buf[4]);
                         return NULL;
                 }
                 *p = '\0'; /* Terminate iSCSI Name */
-                p += 5; /* Skip over ",i,0x" seperator */
+                p += 5; /* Skip over ",i,0x" separator */
 
                 *port_nexus_ptr = p;
                 /*
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index cbb5aaf3e567..0360383dfb94 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -125,6 +125,19 @@ static struct se_device *fd_create_virtdevice(
          * of pure timestamp updates.
          */
         flags = O_RDWR | O_CREAT | O_LARGEFILE | O_DSYNC;
+        /*
+         * Optionally allow fd_buffered_io=1 to be enabled for people
+         * who want use the fs buffer cache as an WriteCache mechanism.
+         *
+         * This means that in event of a hard failure, there is a risk
+         * of silent data-loss if the SCSI client has *not* performed a
+         * forced unit access (FUA) write, or issued SYNCHRONIZE_CACHE
+         * to write-out the entire device cache.
+         */
+        if (fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE) {
+                pr_debug("FILEIO: Disabling O_DSYNC, using buffered FILEIO\n");
+                flags &= ~O_DSYNC;
+        }
 
         file = filp_open(fd_dev->fd_dev_name, flags, 0600);
         if (IS_ERR(file)) {
@@ -188,6 +201,12 @@ static struct se_device *fd_create_virtdevice(
         if (!dev)
                 goto fail;
 
+        if (fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE) {
+                pr_debug("FILEIO: Forcing setting of emulate_write_cache=1"
+                        " with FDBD_HAS_BUFFERED_IO_WCE\n");
+                dev->se_sub_dev->se_dev_attrib.emulate_write_cache = 1;
+        }
+
         fd_dev->fd_dev_id = fd_host->fd_host_dev_id_count++;
         fd_dev->fd_queue_depth = dev->queue_depth;
 
@@ -407,6 +426,7 @@ enum {
 static match_table_t tokens = {
         {Opt_fd_dev_name, "fd_dev_name=%s"},
         {Opt_fd_dev_size, "fd_dev_size=%s"},
+        {Opt_fd_buffered_io, "fd_buffered_io=%d"},
         {Opt_err, NULL}
 };
 
@@ -418,7 +438,7 @@ static ssize_t fd_set_configfs_dev_params(
         struct fd_dev *fd_dev = se_dev->se_dev_su_ptr;
         char *orig, *ptr, *arg_p, *opts;
         substring_t args[MAX_OPT_ARGS];
-        int ret = 0, token;
+        int ret = 0, arg, token;
 
         opts = kstrdup(page, GFP_KERNEL);
         if (!opts)
@@ -459,6 +479,19 @@ static ssize_t fd_set_configfs_dev_params(
                                         " bytes\n", fd_dev->fd_dev_size);
                         fd_dev->fbd_flags |= FBDF_HAS_SIZE;
                         break;
+                case Opt_fd_buffered_io:
+                        match_int(args, &arg);
+                        if (arg != 1) {
+                                pr_err("bogus fd_buffered_io=%d value\n", arg);
+                                ret = -EINVAL;
+                                goto out;
+                        }
+
+                        pr_debug("FILEIO: Using buffered I/O"
+                                " operations for struct fd_dev\n");
+
+                        fd_dev->fbd_flags |= FDBD_HAS_BUFFERED_IO_WCE;
+                        break;
                 default:
                         break;
                 }
@@ -490,8 +523,10 @@ static ssize_t fd_show_configfs_dev_params(
         ssize_t bl = 0;
 
         bl = sprintf(b + bl, "TCM FILEIO ID: %u", fd_dev->fd_dev_id);
-        bl += sprintf(b + bl, "        File: %s  Size: %llu  Mode: O_DSYNC\n",
-                fd_dev->fd_dev_name, fd_dev->fd_dev_size);
+        bl += sprintf(b + bl, "        File: %s  Size: %llu  Mode: %s\n",
+                fd_dev->fd_dev_name, fd_dev->fd_dev_size,
+                (fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE) ?
+                "Buffered-WCE" : "O_DSYNC");
         return bl;
 }
 
@@ -546,8 +581,6 @@ static struct se_subsystem_api fileio_template = {
         .name                   = "fileio",
         .owner                  = THIS_MODULE,
         .transport_type         = TRANSPORT_PLUGIN_VHBA_PDEV,
-        .write_cache_emulated   = 1,
-        .fua_write_emulated     = 1,
         .attach_hba             = fd_attach_hba,
         .detach_hba             = fd_detach_hba,
         .allocate_virtdevice    = fd_allocate_virtdevice,
diff --git a/drivers/target/target_core_file.h b/drivers/target/target_core_file.h
index 70ce7fd7111d..876ae53ef5b8 100644
--- a/drivers/target/target_core_file.h
+++ b/drivers/target/target_core_file.h
@@ -14,6 +14,7 @@
 
 #define FBDF_HAS_PATH           0x01
 #define FBDF_HAS_SIZE           0x02
+#define FDBD_HAS_BUFFERED_IO_WCE 0x04
 
 struct fd_dev {
         u32             fbd_flags;
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 9ba495477fd2..57d7674c5013 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -454,14 +454,11 @@ static ssize_t iblock_set_configfs_dev_params(struct se_hba *hba,
                                 ret = -EEXIST;
                                 goto out;
                         }
-                        arg_p = match_strdup(&args[0]);
-                        if (!arg_p) {
-                                ret = -ENOMEM;
+                        if (match_strlcpy(ib_dev->ibd_udev_path, &args[0],
+                                SE_UDEV_PATH_LEN) == 0) {
+                                ret = -EINVAL;
                                 break;
                         }
-                        snprintf(ib_dev->ibd_udev_path, SE_UDEV_PATH_LEN,
-                                        "%s", arg_p);
-                        kfree(arg_p);
                         pr_debug("IBLOCK: Referencing UDEV path: %s\n",
                                         ib_dev->ibd_udev_path);
                         ib_dev->ibd_flags |= IBDF_HAS_UDEV_PATH;
@@ -556,14 +553,6 @@ static void iblock_complete_cmd(struct se_cmd *cmd)
         kfree(ibr);
 }
 
-static void iblock_bio_destructor(struct bio *bio)
-{
-        struct se_cmd *cmd = bio->bi_private;
-        struct iblock_dev *ib_dev = cmd->se_dev->dev_ptr;
-
-        bio_free(bio, ib_dev->ibd_bio_set);
-}
-
 static struct bio *
 iblock_get_bio(struct se_cmd *cmd, sector_t lba, u32 sg_num)
 {
@@ -585,7 +574,6 @@ iblock_get_bio(struct se_cmd *cmd, sector_t lba, u32 sg_num)
 
         bio->bi_bdev = ib_dev->ibd_bd;
         bio->bi_private = cmd;
-        bio->bi_destructor = iblock_bio_destructor;
         bio->bi_end_io = &iblock_bio_done;
         bio->bi_sector = lba;
         return bio;
@@ -657,6 +645,12 @@ static int iblock_execute_rw(struct se_cmd *cmd)
                 goto fail;
         cmd->priv = ibr;
 
+        if (!sgl_nents) {
+                atomic_set(&ibr->pending, 1);
+                iblock_complete_cmd(cmd);
+                return 0;
+        }
+
         bio = iblock_get_bio(cmd, block_lba, sgl_nents);
         if (!bio)
                 goto fail_free_ibr;
@@ -769,8 +763,6 @@ static struct se_subsystem_api iblock_template = {
         .name                   = "iblock",
         .owner                  = THIS_MODULE,
         .transport_type         = TRANSPORT_PLUGIN_VHBA_PDEV,
-        .write_cache_emulated   = 1,
-        .fua_write_emulated     = 1,
         .attach_hba             = iblock_attach_hba,
         .detach_hba             = iblock_detach_hba,
         .allocate_virtdevice    = iblock_allocate_virtdevice,
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 956c84c6b666..8c323a98c4a0 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -197,10 +197,10 @@ int target_scsi2_reservation_release(struct se_cmd *cmd)
 {
         struct se_device *dev = cmd->se_dev;
         struct se_session *sess = cmd->se_sess;
-        struct se_portal_group *tpg = sess->se_tpg;
+        struct se_portal_group *tpg;
         int ret = 0, rc;
 
-        if (!sess || !tpg)
+        if (!sess || !sess->se_tpg)
                 goto out;
         rc = target_check_scsi2_reservation_conflict(cmd);
         if (rc == 1)
@@ -228,6 +228,7 @@ int target_scsi2_reservation_release(struct se_cmd *cmd)
                 dev->dev_res_bin_isid = 0;
                 dev->dev_flags &= ~DF_SPC2_RESERVATIONS_WITH_ISID;
         }
+        tpg = sess->se_tpg;
         pr_debug("SCSI-2 Released reservation for %s LUN: %u ->"
                 " MAPPED LUN: %u for %s\n", tpg->se_tpg_tfo->get_fabric_name(),
                 cmd->se_lun->unpacked_lun, cmd->se_deve->mapped_lun,
@@ -245,7 +246,7 @@ int target_scsi2_reservation_reserve(struct se_cmd *cmd)
 {
         struct se_device *dev = cmd->se_dev;
         struct se_session *sess = cmd->se_sess;
-        struct se_portal_group *tpg = sess->se_tpg;
+        struct se_portal_group *tpg;
         int ret = 0, rc;
 
         if ((cmd->t_task_cdb[1] & 0x01) &&
@@ -260,7 +261,7 @@ int target_scsi2_reservation_reserve(struct se_cmd *cmd)
          * This is currently the case for target_core_mod passthrough struct se_cmd
          * ops
          */
-        if (!sess || !tpg)
+        if (!sess || !sess->se_tpg)
                 goto out;
         rc = target_check_scsi2_reservation_conflict(cmd);
         if (rc == 1)
@@ -272,6 +273,7 @@ int target_scsi2_reservation_reserve(struct se_cmd *cmd)
         }
 
         ret = 0;
+        tpg = sess->se_tpg;
         spin_lock(&dev->dev_reservation_lock);
         if (dev->dev_reserved_node_acl &&
            (dev->dev_reserved_node_acl != sess->se_node_acl)) {
@@ -1620,7 +1622,7 @@ static int core_scsi3_decode_spec_i_port(
                                 goto out;
                         }
                         /*
-                         * Locate the desination initiator ACL to be registered
+                         * Locate the destination initiator ACL to be registered
                          * from the decoded fabric module specific TransportID
                          * at *i_str.
                          */
@@ -4257,7 +4259,7 @@ static int core_scsi3_pri_read_full_status(struct se_cmd *cmd)
                         buf[off++] = ((port->sep_rtpi >> 8) & 0xff);
                         buf[off++] = (port->sep_rtpi & 0xff);
                 } else
-                        off += 2; /* Skip over RELATIVE TARGET PORT IDENTIFER */
+                        off += 2; /* Skip over RELATIVE TARGET PORT IDENTIFIER */
 
                 /*
                  * Now, have the $FABRIC_MOD fill in the protocol identifier
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 9d7ce3daa262..617c086a8a02 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -264,7 +264,7 @@ pscsi_get_inquiry_vpd_device_ident(struct scsi_device *sdev,
                                         " length zero!\n");
                         break;
                 }
-                pr_debug("T10 VPD Identifer Length: %d\n", ident_len);
+                pr_debug("T10 VPD Identifier Length: %d\n", ident_len);
 
                 vpd = kzalloc(sizeof(struct t10_vpd), GFP_KERNEL);
                 if (!vpd) {
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index a9dd9469e3bd..a6e27d967c7b 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -40,8 +40,9 @@
 static int sbc_emulate_readcapacity(struct se_cmd *cmd)
 {
         struct se_device *dev = cmd->se_dev;
-        unsigned char *buf;
         unsigned long long blocks_long = dev->transport->get_blocks(dev);
+        unsigned char *rbuf;
+        unsigned char buf[8];
         u32 blocks;
 
         if (blocks_long >= 0x00000000ffffffff)
@@ -49,8 +50,6 @@ static int sbc_emulate_readcapacity(struct se_cmd *cmd)
         else
                 blocks = (u32)blocks_long;
 
-        buf = transport_kmap_data_sg(cmd);
-
         buf[0] = (blocks >> 24) & 0xff;
         buf[1] = (blocks >> 16) & 0xff;
         buf[2] = (blocks >> 8) & 0xff;
@@ -60,7 +59,11 @@ static int sbc_emulate_readcapacity(struct se_cmd *cmd)
         buf[6] = (dev->se_sub_dev->se_dev_attrib.block_size >> 8) & 0xff;
         buf[7] = dev->se_sub_dev->se_dev_attrib.block_size & 0xff;
 
-        transport_kunmap_data_sg(cmd);
+        rbuf = transport_kmap_data_sg(cmd);
+        if (rbuf) {
+                memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+                transport_kunmap_data_sg(cmd);
+        }
 
         target_complete_cmd(cmd, GOOD);
         return 0;
@@ -69,11 +72,11 @@ static int sbc_emulate_readcapacity(struct se_cmd *cmd)
 static int sbc_emulate_readcapacity_16(struct se_cmd *cmd)
 {
         struct se_device *dev = cmd->se_dev;
-        unsigned char *buf;
+        unsigned char *rbuf;
+        unsigned char buf[32];
         unsigned long long blocks = dev->transport->get_blocks(dev);
 
-        buf = transport_kmap_data_sg(cmd);
-
+        memset(buf, 0, sizeof(buf));
         buf[0] = (blocks >> 56) & 0xff;
         buf[1] = (blocks >> 48) & 0xff;
         buf[2] = (blocks >> 40) & 0xff;
@@ -93,7 +96,11 @@ static int sbc_emulate_readcapacity_16(struct se_cmd *cmd)
         if (dev->se_sub_dev->se_dev_attrib.emulate_tpu || dev->se_sub_dev->se_dev_attrib.emulate_tpws)
                 buf[14] = 0x80;
 
-        transport_kunmap_data_sg(cmd);
+        rbuf = transport_kmap_data_sg(cmd);
+        if (rbuf) {
+                memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+                transport_kunmap_data_sg(cmd);
+        }
 
         target_complete_cmd(cmd, GOOD);
         return 0;
@@ -128,6 +135,12 @@ static int sbc_emulate_verify(struct se_cmd *cmd)
         return 0;
 }
 
+static int sbc_emulate_noop(struct se_cmd *cmd)
+{
+        target_complete_cmd(cmd, GOOD);
+        return 0;
+}
+
 static inline u32 sbc_get_size(struct se_cmd *cmd, u32 sectors)
 {
         return cmd->se_dev->se_sub_dev->se_dev_attrib.block_size * sectors;
@@ -524,6 +537,18 @@ int sbc_parse_cdb(struct se_cmd *cmd, struct spc_ops *ops)
                 size = 0;
                 cmd->execute_cmd = sbc_emulate_verify;
                 break;
+        case REZERO_UNIT:
+        case SEEK_6:
+        case SEEK_10:
+                /*
+                 * There are still clients out there which use these old SCSI-2
+                 * commands. This mainly happens when running VMs with legacy
+                 * guest systems, connected via SCSI command pass-through to
+                 * iSCSI targets. Make them happy and return status GOOD.
+                 */
+                size = 0;
+                cmd->execute_cmd = sbc_emulate_noop;
+                break;
         default:
                 ret = spc_parse_cdb(cmd, &size);
                 if (ret)
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 388a922c8f6d..6fd434d3d7e4 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -600,29 +600,12 @@ static int spc_emulate_inquiry(struct se_cmd *cmd)
 {
         struct se_device *dev = cmd->se_dev;
         struct se_portal_group *tpg = cmd->se_lun->lun_sep->sep_tpg;
-        unsigned char *buf, *map_buf;
+        unsigned char *rbuf;
         unsigned char *cdb = cmd->t_task_cdb;
+        unsigned char buf[SE_INQUIRY_BUF];
         int p, ret;
 
-        map_buf = transport_kmap_data_sg(cmd);
-        /*
-         * If SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC is not set, then we
-         * know we actually allocated a full page.  Otherwise, if the
-         * data buffer is too small, allocate a temporary buffer so we
-         * don't have to worry about overruns in all our INQUIRY
-         * emulation handling.
-         */
-        if (cmd->data_length < SE_INQUIRY_BUF &&
-            (cmd->se_cmd_flags & SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC)) {
-                buf = kzalloc(SE_INQUIRY_BUF, GFP_KERNEL);
-                if (!buf) {
-                        transport_kunmap_data_sg(cmd);
-                        cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-                        return -ENOMEM;
-                }
-        } else {
-                buf = map_buf;
-        }
+        memset(buf, 0, SE_INQUIRY_BUF);
 
         if (dev == tpg->tpg_virt_lun0.lun_se_dev)
                 buf[0] = 0x3f; /* Not connected */
@@ -655,11 +638,11 @@ static int spc_emulate_inquiry(struct se_cmd *cmd)
         ret = -EINVAL;
 
 out:
-        if (buf != map_buf) {
-                memcpy(map_buf, buf, cmd->data_length);
-                kfree(buf);
+        rbuf = transport_kmap_data_sg(cmd);
+        if (rbuf) {
+                memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+                transport_kunmap_data_sg(cmd);
         }
-        transport_kunmap_data_sg(cmd);
 
         if (!ret)
                 target_complete_cmd(cmd, GOOD);
@@ -803,7 +786,7 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
         unsigned char *rbuf;
         int type = dev->transport->get_device_type(dev);
         int ten = (cmd->t_task_cdb[0] == MODE_SENSE_10);
-        int offset = ten ? 8 : 4;
+        u32 offset = ten ? 8 : 4;
         int length = 0;
         unsigned char buf[SE_MODE_PAGE_BUF];
 
@@ -836,6 +819,7 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
                 offset -= 2;
                 buf[0] = (offset >> 8) & 0xff;
                 buf[1] = offset & 0xff;
+                offset += 2;
 
                 if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
                     (cmd->se_deve &&
@@ -845,13 +829,10 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
                 if ((dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0) &&
                     (dev->se_sub_dev->se_dev_attrib.emulate_fua_write > 0))
                         spc_modesense_dpofua(&buf[3], type);
-
-                if ((offset + 2) > cmd->data_length)
-                        offset = cmd->data_length;
-
         } else {
                 offset -= 1;
                 buf[0] = offset & 0xff;
+                offset += 1;
 
                 if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
                     (cmd->se_deve &&
@@ -861,14 +842,13 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
                 if ((dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0) &&
                     (dev->se_sub_dev->se_dev_attrib.emulate_fua_write > 0))
                         spc_modesense_dpofua(&buf[2], type);
-
-                if ((offset + 1) > cmd->data_length)
-                        offset = cmd->data_length;
         }
 
         rbuf = transport_kmap_data_sg(cmd);
-        memcpy(rbuf, buf, offset);
-        transport_kunmap_data_sg(cmd);
+        if (rbuf) {
+                memcpy(rbuf, buf, min(offset, cmd->data_length));
+                transport_kunmap_data_sg(cmd);
+        }
 
         target_complete_cmd(cmd, GOOD);
         return 0;
diff --git a/drivers/target/target_core_stat.c b/drivers/target/target_core_stat.c
index 3d44beb0cf1f..cb6b0036ae95 100644
--- a/drivers/target/target_core_stat.c
+++ b/drivers/target/target_core_stat.c
@@ -32,7 +32,6 @@
 #include <linux/delay.h>
 #include <linux/timer.h>
 #include <linux/string.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index 1c59a3c23b2c..be75c4331a92 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -140,15 +140,15 @@ void core_tmr_abort_task(
                 printk("ABORT_TASK: Found referenced %s task_tag: %u\n",
                         se_cmd->se_tfo->get_fabric_name(), ref_tag);
 
-                spin_lock_irq(&se_cmd->t_state_lock);
+                spin_lock(&se_cmd->t_state_lock);
                 if (se_cmd->transport_state & CMD_T_COMPLETE) {
                         printk("ABORT_TASK: ref_tag: %u already complete, skipping\n", ref_tag);
-                        spin_unlock_irq(&se_cmd->t_state_lock);
+                        spin_unlock(&se_cmd->t_state_lock);
                         spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
                         goto out;
                 }
                 se_cmd->transport_state |= CMD_T_ABORTED;
-                spin_unlock_irq(&se_cmd->t_state_lock);
+                spin_unlock(&se_cmd->t_state_lock);
 
                 list_del_init(&se_cmd->se_cmd_list);
                 kref_get(&se_cmd->cmd_kref);
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index b8628a5014b9..a531fe282b1e 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -303,7 +303,7 @@ struct se_node_acl *core_tpg_check_initiator_node_acl(
         }
         /*
          * Here we only create demo-mode MappedLUNs from the active
-         * TPG LUNs if the fabric is not explictly asking for
+         * TPG LUNs if the fabric is not explicitly asking for
          * tpg_check_demo_mode_login_only() == 1.
          */
         if ((tpg->se_tpg_tfo->tpg_check_demo_mode_login_only == NULL) ||
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 269f54488397..9097155e9ebe 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -55,8 +55,6 @@
 #include "target_core_pr.h"
 #include "target_core_ua.h"
 
-static int sub_api_initialized;
-
 static struct workqueue_struct *target_completion_wq;
 static struct kmem_cache *se_sess_cache;
 struct kmem_cache *se_ua_cache;
@@ -195,6 +193,7 @@ u32 scsi_get_new_index(scsi_index_t type)
 void transport_subsystem_check_init(void)
 {
         int ret;
+        static int sub_api_initialized;
 
         if (sub_api_initialized)
                 return;
@@ -211,12 +210,7 @@ void transport_subsystem_check_init(void)
         if (ret != 0)
                 pr_err("Unable to load target_core_pscsi\n");
 
-        ret = request_module("target_core_stgt");
-        if (ret != 0)
-                pr_err("Unable to load target_core_stgt\n");
-
         sub_api_initialized = 1;
-        return;
 }
 
 struct se_session *transport_init_session(void)
@@ -573,9 +567,7 @@ static void target_complete_failure_work(struct work_struct *work)
  */
 static unsigned char *transport_get_sense_buffer(struct se_cmd *cmd)
 {
-        unsigned char *buffer = cmd->sense_buffer;
         struct se_device *dev = cmd->se_dev;
-        u32 offset = 0;
 
         WARN_ON(!cmd->se_lun);
 
@@ -585,14 +577,11 @@ static unsigned char *transport_get_sense_buffer(struct se_cmd *cmd)
         if (cmd->se_cmd_flags & SCF_SENT_CHECK_CONDITION)
                 return NULL;
 
-        offset = cmd->se_tfo->set_fabric_sense_len(cmd, TRANSPORT_SENSE_BUFFER);
-
-        /* Automatically padded */
-        cmd->scsi_sense_length = TRANSPORT_SENSE_BUFFER + offset;
+        cmd->scsi_sense_length = TRANSPORT_SENSE_BUFFER;
 
         pr_debug("HBA_[%u]_PLUG[%s]: Requesting sense for SAM STATUS: 0x%02x\n",
                 dev->se_hba->hba_id, dev->transport->name, cmd->scsi_status);
-        return &buffer[offset];
+        return cmd->sense_buffer;
 }
 
 void target_complete_cmd(struct se_cmd *cmd, u8 scsi_status)
@@ -969,7 +958,7 @@ int
 transport_set_vpd_ident(struct t10_vpd *vpd, unsigned char *page_83)
 {
         static const char hex_str[] = "0123456789abcdef";
-        int j = 0, i = 4; /* offset to start of the identifer */
+        int j = 0, i = 4; /* offset to start of the identifier */
 
         /*
          * The VPD Code Set (encoding)
@@ -1466,8 +1455,9 @@ int transport_handle_cdb_direct(
 }
 EXPORT_SYMBOL(transport_handle_cdb_direct);
 
-/**
- * target_submit_cmd - lookup unpacked lun and submit uninitialized se_cmd
+/*
+ * target_submit_cmd_map_sgls - lookup unpacked lun and submit uninitialized
+ *                       se_cmd + use pre-allocated SGL memory.
  *
  * @se_cmd: command descriptor to submit
  * @se_sess: associated se_sess for endpoint
@@ -1478,6 +1468,10 @@ EXPORT_SYMBOL(transport_handle_cdb_direct);
  * @task_addr: SAM task attribute
  * @data_dir: DMA data direction
  * @flags: flags for command submission from target_sc_flags_tables
+ * @sgl: struct scatterlist memory for unidirectional mapping
+ * @sgl_count: scatterlist count for unidirectional mapping
+ * @sgl_bidi: struct scatterlist memory for bidirectional READ mapping
+ * @sgl_bidi_count: scatterlist count for bidirectional READ mapping
  *
  * Returns non zero to signal active I/O shutdown failure.  All other
  * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
@@ -1485,10 +1479,12 @@ EXPORT_SYMBOL(transport_handle_cdb_direct);
  *
  * This may only be called from process context, and also currently
  * assumes internal allocation of fabric payload buffer by target-core.
- **/
-int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
+ */
+int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess,
                 unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
-                u32 data_length, int task_attr, int data_dir, int flags)
+                u32 data_length, int task_attr, int data_dir, int flags,
+                struct scatterlist *sgl, u32 sgl_count,
+                struct scatterlist *sgl_bidi, u32 sgl_bidi_count)
 {
         struct se_portal_group *se_tpg;
         int rc;
@@ -1535,7 +1531,42 @@ int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
                 transport_generic_request_failure(se_cmd);
                 return 0;
         }
+        /*
+         * When a non zero sgl_count has been passed perform SGL passthrough
+         * mapping for pre-allocated fabric memory instead of having target
+         * core perform an internal SGL allocation..
+         */
+        if (sgl_count != 0) {
+                BUG_ON(!sgl);
+
+                /*
+                 * A work-around for tcm_loop as some userspace code via
+                 * scsi-generic do not memset their associated read buffers,
+                 * so go ahead and do that here for type non-data CDBs.  Also
+                 * note that this is currently guaranteed to be a single SGL
+                 * for this case by target core in target_setup_cmd_from_cdb()
+                 * -> transport_generic_cmd_sequencer().
+                 */
+                if (!(se_cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) &&
+                     se_cmd->data_direction == DMA_FROM_DEVICE) {
+                        unsigned char *buf = NULL;
+
+                        if (sgl)
+                                buf = kmap(sg_page(sgl)) + sgl->offset;
+
+                        if (buf) {
+                                memset(buf, 0, sgl->length);
+                                kunmap(sg_page(sgl));
+                        }
+                }
 
+                rc = transport_generic_map_mem_to_cmd(se_cmd, sgl, sgl_count,
+                                sgl_bidi, sgl_bidi_count);
+                if (rc != 0) {
+                        transport_generic_request_failure(se_cmd);
+                        return 0;
+                }
+        }
         /*
          * Check if we need to delay processing because of ALUA
          * Active/NonOptimized primary access state..
@@ -1545,6 +1576,38 @@ int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
         transport_handle_cdb_direct(se_cmd);
         return 0;
 }
+EXPORT_SYMBOL(target_submit_cmd_map_sgls);
+
+/*
+ * target_submit_cmd - lookup unpacked lun and submit uninitialized se_cmd
+ *
+ * @se_cmd: command descriptor to submit
+ * @se_sess: associated se_sess for endpoint
+ * @cdb: pointer to SCSI CDB
+ * @sense: pointer to SCSI sense buffer
+ * @unpacked_lun: unpacked LUN to reference for struct se_lun
+ * @data_length: fabric expected data transfer length
+ * @task_addr: SAM task attribute
+ * @data_dir: DMA data direction
+ * @flags: flags for command submission from target_sc_flags_tables
+ *
+ * Returns non zero to signal active I/O shutdown failure.  All other
+ * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
+ * but still return zero here.
+ *
+ * This may only be called from process context, and also currently
+ * assumes internal allocation of fabric payload buffer by target-core.
+ *
+ * It also assumes interal target core SGL memory allocation.
+ */
+int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
+                unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
+                u32 data_length, int task_attr, int data_dir, int flags)
+{
+        return target_submit_cmd_map_sgls(se_cmd, se_sess, cdb, sense,
+                        unpacked_lun, data_length, task_attr, data_dir,
+                        flags, NULL, 0, NULL, 0);
+}
 EXPORT_SYMBOL(target_submit_cmd);
 
 static void target_complete_tmr_failure(struct work_struct *work)
@@ -1553,7 +1616,6 @@ static void target_complete_tmr_failure(struct work_struct *work)
 
         se_cmd->se_tmr_req->response = TMR_LUN_DOES_NOT_EXIST;
         se_cmd->se_tfo->queue_tm_rsp(se_cmd);
-        transport_generic_free_cmd(se_cmd, 0);
 }
 
 /**
@@ -2300,23 +2362,6 @@ int transport_generic_new_cmd(struct se_cmd *cmd)
                 if (ret < 0)
                         goto out_fail;
         }
-        /*
-         * If this command doesn't have any payload and we don't have to call
-         * into the fabric for data transfers, go ahead and complete it right
-         * away.
-         */
-        if (!cmd->data_length &&
-            cmd->t_task_cdb[0] != REQUEST_SENSE &&
-            cmd->se_dev->transport->transport_type != TRANSPORT_PLUGIN_PHBA_PDEV) {
-                spin_lock_irq(&cmd->t_state_lock);
-                cmd->t_state = TRANSPORT_COMPLETE;
-                cmd->transport_state |= CMD_T_ACTIVE;
-                spin_unlock_irq(&cmd->t_state_lock);
-
-                INIT_WORK(&cmd->work, target_complete_ok_work);
-                queue_work(target_completion_wq, &cmd->work);
-                return 0;
-        }
 
         atomic_inc(&cmd->t_fe_count);
 
@@ -2771,7 +2816,7 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
         spin_lock_irqsave(&cmd->t_state_lock, flags);
         cmd->transport_state &= ~(CMD_T_ACTIVE | CMD_T_STOP);
 
-        pr_debug("wait_for_tasks: Stopped wait_for_compltion("
+        pr_debug("wait_for_tasks: Stopped wait_for_completion("
                 "&cmd->t_transport_stop_comp) for ITT: 0x%08x\n",
                 cmd->se_tfo->get_task_tag(cmd));
 
@@ -2810,7 +2855,6 @@ int transport_send_check_condition_and_sense(
 {
         unsigned char *buffer = cmd->sense_buffer;
         unsigned long flags;
-        int offset;
         u8 asc = 0, ascq = 0;
 
         spin_lock_irqsave(&cmd->t_state_lock, flags);
@@ -2826,14 +2870,7 @@ int transport_send_check_condition_and_sense(
 
         if (!from_transport)
                 cmd->se_cmd_flags |= SCF_EMULATED_TASK_SENSE;
-        /*
-         * Data Segment and SenseLength of the fabric response PDU.
-         *
-         * TRANSPORT_SENSE_BUFFER is now set to SCSI_SENSE_BUFFERSIZE
-         * from include/scsi/scsi_cmnd.h
-         */
-        offset = cmd->se_tfo->set_fabric_sense_len(cmd,
-                                TRANSPORT_SENSE_BUFFER);
+
         /*
          * Actual SENSE DATA, see SPC-3 7.23.2  SPC_SENSE_KEY_OFFSET uses
          * SENSE KEY values from include/scsi/scsi.h
@@ -2841,151 +2878,151 @@ int transport_send_check_condition_and_sense(
         switch (reason) {
         case TCM_NON_EXISTENT_LUN:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* LOGICAL UNIT NOT SUPPORTED */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x25;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x25;
                 break;
         case TCM_UNSUPPORTED_SCSI_OPCODE:
         case TCM_SECTOR_COUNT_TOO_MANY:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* INVALID COMMAND OPERATION CODE */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x20;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x20;
                 break;
         case TCM_UNKNOWN_MODE_PAGE:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* INVALID FIELD IN CDB */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x24;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x24;
                 break;
         case TCM_CHECK_CONDITION_ABORT_CMD:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ABORTED COMMAND */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+                buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
                 /* BUS DEVICE RESET FUNCTION OCCURRED */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x29;
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x03;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x29;
+                buffer[SPC_ASCQ_KEY_OFFSET] = 0x03;
                 break;
         case TCM_INCORRECT_AMOUNT_OF_DATA:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ABORTED COMMAND */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+                buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
                 /* WRITE ERROR */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x0c;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x0c;
                 /* NOT ENOUGH UNSOLICITED DATA */
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x0d;
+                buffer[SPC_ASCQ_KEY_OFFSET] = 0x0d;
                 break;
         case TCM_INVALID_CDB_FIELD:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* INVALID FIELD IN CDB */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x24;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x24;
                 break;
         case TCM_INVALID_PARAMETER_LIST:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* INVALID FIELD IN PARAMETER LIST */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x26;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x26;
                 break;
         case TCM_UNEXPECTED_UNSOLICITED_DATA:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ABORTED COMMAND */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+                buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
                 /* WRITE ERROR */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x0c;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x0c;
                 /* UNEXPECTED_UNSOLICITED_DATA */
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x0c;
+                buffer[SPC_ASCQ_KEY_OFFSET] = 0x0c;
                 break;
         case TCM_SERVICE_CRC_ERROR:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ABORTED COMMAND */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+                buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
                 /* PROTOCOL SERVICE CRC ERROR */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x47;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x47;
                 /* N/A */
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x05;
+                buffer[SPC_ASCQ_KEY_OFFSET] = 0x05;
                 break;
         case TCM_SNACK_REJECTED:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ABORTED COMMAND */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+                buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
                 /* READ ERROR */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x11;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x11;
                 /* FAILED RETRANSMISSION REQUEST */
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x13;
+                buffer[SPC_ASCQ_KEY_OFFSET] = 0x13;
                 break;
         case TCM_WRITE_PROTECTED:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* DATA PROTECT */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = DATA_PROTECT;
+                buffer[SPC_SENSE_KEY_OFFSET] = DATA_PROTECT;
                 /* WRITE PROTECTED */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x27;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x27;
                 break;
         case TCM_ADDRESS_OUT_OF_RANGE:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* LOGICAL BLOCK ADDRESS OUT OF RANGE */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x21;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x21;
                 break;
         case TCM_CHECK_CONDITION_UNIT_ATTENTION:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* UNIT ATTENTION */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = UNIT_ATTENTION;
+                buffer[SPC_SENSE_KEY_OFFSET] = UNIT_ATTENTION;
                 core_scsi3_ua_for_check_condition(cmd, &asc, &ascq);
-                buffer[offset+SPC_ASC_KEY_OFFSET] = asc;
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = ascq;
+                buffer[SPC_ASC_KEY_OFFSET] = asc;
+                buffer[SPC_ASCQ_KEY_OFFSET] = ascq;
                 break;
         case TCM_CHECK_CONDITION_NOT_READY:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* Not Ready */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = NOT_READY;
+                buffer[SPC_SENSE_KEY_OFFSET] = NOT_READY;
                 transport_get_sense_codes(cmd, &asc, &ascq);
-                buffer[offset+SPC_ASC_KEY_OFFSET] = asc;
-                buffer[offset+SPC_ASCQ_KEY_OFFSET] = ascq;
+                buffer[SPC_ASC_KEY_OFFSET] = asc;
+                buffer[SPC_ASCQ_KEY_OFFSET] = ascq;
                 break;
         case TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE:
         default:
                 /* CURRENT ERROR */
-                buffer[offset] = 0x70;
-                buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+                buffer[0] = 0x70;
+                buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
                 /* ILLEGAL REQUEST */
-                buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+                buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
                 /* LOGICAL UNIT COMMUNICATION FAILURE */
-                buffer[offset+SPC_ASC_KEY_OFFSET] = 0x80;
+                buffer[SPC_ASC_KEY_OFFSET] = 0x80;
                 break;
         }
         /*
@@ -2996,7 +3033,7 @@ int transport_send_check_condition_and_sense(
          * Automatically padded, this value is encoded in the fabric's
          * data_length response PDU containing the SCSI defined sense data.
          */
-        cmd->scsi_sense_length  = TRANSPORT_SENSE_BUFFER + offset;
+        cmd->scsi_sense_length  = TRANSPORT_SENSE_BUFFER;
 
 after_reason:
         return cmd->se_tfo->queue_status(cmd);
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index 823e6922249d..b406f178ff39 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -19,7 +19,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/slab.h>
diff --git a/drivers/target/tcm_fc/tfc_conf.c b/drivers/target/tcm_fc/tfc_conf.c
index 9501844fae2d..b74feb0d5133 100644
--- a/drivers/target/tcm_fc/tfc_conf.c
+++ b/drivers/target/tcm_fc/tfc_conf.c
@@ -495,16 +495,6 @@ static void ft_set_default_node_attr(struct se_node_acl *se_nacl)
 {
 }
 
-static u16 ft_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
-static u16 ft_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_len)
-{
-        return 0;
-}
-
 static u32 ft_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
         struct ft_tpg *tpg = se_tpg->se_tpg_fabric_ptr;
@@ -542,8 +532,6 @@ static struct target_core_fabric_ops ft_fabric_ops = {
         .queue_data_in =                ft_queue_data_in,
         .queue_status =                 ft_queue_status,
         .queue_tm_rsp =                 ft_queue_tm_resp,
-        .get_fabric_sense_len =         ft_get_fabric_sense_len,
-        .set_fabric_sense_len =         ft_set_fabric_sense_len,
         /*
          * Setup function pointers for generic logic in
          * target_core_fabric_configfs.c
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index ad36ede1a1ea..b6fd4cf42840 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -28,7 +28,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -328,11 +327,12 @@ drop:
  */
 void ft_invl_hw_context(struct ft_cmd *cmd)
 {
-        struct fc_seq *seq = cmd->seq;
+        struct fc_seq *seq;
         struct fc_exch *ep = NULL;
         struct fc_lport *lport = NULL;
 
         BUG_ON(!cmd);
+        seq = cmd->seq;
 
         /* Cleanup the DDP context in HW if DDP was setup */
         if (cmd->was_ddp_setup && seq) {
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index 3c9e5b57caab..9585010964ec 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -19,7 +19,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/slab.h>
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index 3ab2bd540b54..e1cb6bd75f60 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -19,6 +19,18 @@ config THERMAL_HWMON
         depends on HWMON=y || HWMON=THERMAL
         default y
 
+config CPU_THERMAL
+        bool "generic cpu cooling support"
+        depends on THERMAL && CPU_FREQ
+        select CPU_FREQ_TABLE
+        help
+          This implements the generic cpu cooling mechanism through frequency
+          reduction, cpu hotplug and any other ways of reducing temperature. An
+          ACPI version of this already exists(drivers/acpi/processor_thermal.c).
+          This will be useful for platforms using the generic thermal interface
+          and not the ACPI interface.
+          If you want this support, you should say Y here.
+
 config SPEAR_THERMAL
         bool "SPEAr thermal sensor driver"
         depends on THERMAL
@@ -27,3 +39,19 @@ config SPEAR_THERMAL
         help
           Enable this to plug the SPEAr thermal sensor driver into the Linux
           thermal framework
+
+config RCAR_THERMAL
+        tristate "Renesas R-Car thermal driver"
+        depends on THERMAL
+        depends on ARCH_SHMOBILE
+        help
+          Enable this to plug the R-Car thermal sensor driver into the Linux
+          thermal framework
+
+config EXYNOS_THERMAL
+        tristate "Temperature sensor on Samsung EXYNOS"
+        depends on (ARCH_EXYNOS4 || ARCH_EXYNOS5) && THERMAL
+        select CPU_FREQ_TABLE
+        help
+          If you say yes here you get support for TMU (Thermal Managment
+          Unit) on SAMSUNG EXYNOS series of SoC.
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index a9fff0bf4b14..885550dc64b7 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -3,4 +3,7 @@
 #
 
 obj-$(CONFIG_THERMAL)           += thermal_sys.o
-obj-$(CONFIG_SPEAR_THERMAL)             += spear_thermal.o
\ No newline at end of file
+obj-$(CONFIG_CPU_THERMAL)               += cpu_cooling.o
+obj-$(CONFIG_SPEAR_THERMAL)             += spear_thermal.o
+obj-$(CONFIG_RCAR_THERMAL)      += rcar_thermal.o
+obj-$(CONFIG_EXYNOS_THERMAL)            += exynos_thermal.o
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
new file mode 100644
index 000000000000..cc1c930a90e4
--- /dev/null
+++ b/drivers/thermal/cpu_cooling.c
@@ -0,0 +1,449 @@
+/*
+ *  linux/drivers/thermal/cpu_cooling.c
+ *
+ *  Copyright (C) 2012  Samsung Electronics Co., Ltd(http://www.samsung.com)
+ *  Copyright (C) 2012  Amit Daniel <amit.kachhap@linaro.org>
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *  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; version 2 of the License.
+ *
+ *  This program is distributed in the hope that it will be useful, but
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/thermal.h>
+#include <linux/platform_device.h>
+#include <linux/cpufreq.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/cpu.h>
+#include <linux/cpu_cooling.h>
+
+/**
+ * struct cpufreq_cooling_device
+ * @id: unique integer value corresponding to each cpufreq_cooling_device
+ *      registered.
+ * @cool_dev: thermal_cooling_device pointer to keep track of the the
+ *      egistered cooling device.
+ * @cpufreq_state: integer value representing the current state of cpufreq
+ *      cooling devices.
+ * @cpufreq_val: integer value representing the absolute value of the clipped
+ *      frequency.
+ * @allowed_cpus: all the cpus involved for this cpufreq_cooling_device.
+ * @node: list_head to link all cpufreq_cooling_device together.
+ *
+ * This structure is required for keeping information of each
+ * cpufreq_cooling_device registered as a list whose head is represented by
+ * cooling_cpufreq_list. In order to prevent corruption of this list a
+ * mutex lock cooling_cpufreq_lock is used.
+ */
+struct cpufreq_cooling_device {
+        int id;
+        struct thermal_cooling_device *cool_dev;
+        unsigned int cpufreq_state;
+        unsigned int cpufreq_val;
+        struct cpumask allowed_cpus;
+        struct list_head node;
+};
+static LIST_HEAD(cooling_cpufreq_list);
+static DEFINE_IDR(cpufreq_idr);
+
+static struct mutex cooling_cpufreq_lock;
+
+/* notify_table passes value to the CPUFREQ_ADJUST callback function. */
+#define NOTIFY_INVALID NULL
+struct cpufreq_cooling_device *notify_device;
+
+/**
+ * get_idr - function to get a unique id.
+ * @idr: struct idr * handle used to create a id.
+ * @id: int * value generated by this function.
+ */
+static int get_idr(struct idr *idr, int *id)
+{
+        int err;
+again:
+        if (unlikely(idr_pre_get(idr, GFP_KERNEL) == 0))
+                return -ENOMEM;
+
+        mutex_lock(&cooling_cpufreq_lock);
+        err = idr_get_new(idr, NULL, id);
+        mutex_unlock(&cooling_cpufreq_lock);
+
+        if (unlikely(err == -EAGAIN))
+                goto again;
+        else if (unlikely(err))
+                return err;
+
+        *id = *id & MAX_IDR_MASK;
+        return 0;
+}
+
+/**
+ * release_idr - function to free the unique id.
+ * @idr: struct idr * handle used for creating the id.
+ * @id: int value representing the unique id.
+ */
+static void release_idr(struct idr *idr, int id)
+{
+        mutex_lock(&cooling_cpufreq_lock);
+        idr_remove(idr, id);
+        mutex_unlock(&cooling_cpufreq_lock);
+}
+
+/* Below code defines functions to be used for cpufreq as cooling device */
+
+/**
+ * is_cpufreq_valid - function to check if a cpu has frequency transition policy.
+ * @cpu: cpu for which check is needed.
+ */
+static int is_cpufreq_valid(int cpu)
+{
+        struct cpufreq_policy policy;
+        return !cpufreq_get_policy(&policy, cpu);
+}
+
+/**
+ * get_cpu_frequency - get the absolute value of frequency from level.
+ * @cpu: cpu for which frequency is fetched.
+ * @level: level of frequency of the CPU
+ *      e.g level=1 --> 1st MAX FREQ, LEVEL=2 ---> 2nd MAX FREQ, .... etc
+ */
+static unsigned int get_cpu_frequency(unsigned int cpu, unsigned long level)
+{
+        int ret = 0, i = 0;
+        unsigned long level_index;
+        bool descend = false;
+        struct cpufreq_frequency_table *table =
+                                        cpufreq_frequency_get_table(cpu);
+        if (!table)
+                return ret;
+
+        while (table[i].frequency != CPUFREQ_TABLE_END) {
+                if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+                        continue;
+
+                /*check if table in ascending or descending order*/
+                if ((table[i + 1].frequency != CPUFREQ_TABLE_END) &&
+                        (table[i + 1].frequency < table[i].frequency)
+                        && !descend) {
+                        descend = true;
+                }
+
+                /*return if level matched and table in descending order*/
+                if (descend && i == level)
+                        return table[i].frequency;
+                i++;
+        }
+        i--;
+
+        if (level > i || descend)
+                return ret;
+        level_index = i - level;
+
+        /*Scan the table in reverse order and match the level*/
+        while (i >= 0) {
+                if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+                        continue;
+                /*return if level matched*/
+                if (i == level_index)
+                        return table[i].frequency;
+                i--;
+        }
+        return ret;
+}
+
+/**
+ * cpufreq_apply_cooling - function to apply frequency clipping.
+ * @cpufreq_device: cpufreq_cooling_device pointer containing frequency
+ *      clipping data.
+ * @cooling_state: value of the cooling state.
+ */
+static int cpufreq_apply_cooling(struct cpufreq_cooling_device *cpufreq_device,
+                                unsigned long cooling_state)
+{
+        unsigned int cpuid, clip_freq;
+        struct cpumask *maskPtr = &cpufreq_device->allowed_cpus;
+        unsigned int cpu = cpumask_any(maskPtr);
+
+
+        /* Check if the old cooling action is same as new cooling action */
+        if (cpufreq_device->cpufreq_state == cooling_state)
+                return 0;
+
+        clip_freq = get_cpu_frequency(cpu, cooling_state);
+        if (!clip_freq)
+                return -EINVAL;
+
+        cpufreq_device->cpufreq_state = cooling_state;
+        cpufreq_device->cpufreq_val = clip_freq;
+        notify_device = cpufreq_device;
+
+        for_each_cpu(cpuid, maskPtr) {
+                if (is_cpufreq_valid(cpuid))
+                        cpufreq_update_policy(cpuid);
+        }
+
+        notify_device = NOTIFY_INVALID;
+
+        return 0;
+}
+
+/**
+ * cpufreq_thermal_notifier - notifier callback for cpufreq policy change.
+ * @nb: struct notifier_block * with callback info.
+ * @event: value showing cpufreq event for which this function invoked.
+ * @data: callback-specific data
+ */
+static int cpufreq_thermal_notifier(struct notifier_block *nb,
+                                        unsigned long event, void *data)
+{
+        struct cpufreq_policy *policy = data;
+        unsigned long max_freq = 0;
+
+        if (event != CPUFREQ_ADJUST || notify_device == NOTIFY_INVALID)
+                return 0;
+
+        if (cpumask_test_cpu(policy->cpu, &notify_device->allowed_cpus))
+                max_freq = notify_device->cpufreq_val;
+
+        /* Never exceed user_policy.max*/
+        if (max_freq > policy->user_policy.max)
+                max_freq = policy->user_policy.max;
+
+        if (policy->max != max_freq)
+                cpufreq_verify_within_limits(policy, 0, max_freq);
+
+        return 0;
+}
+
+/*
+ * cpufreq cooling device callback functions are defined below
+ */
+
+/**
+ * cpufreq_get_max_state - callback function to get the max cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: fill this variable with the max cooling state.
+ */
+static int cpufreq_get_max_state(struct thermal_cooling_device *cdev,
+                                 unsigned long *state)
+{
+        int ret = -EINVAL, i = 0;
+        struct cpufreq_cooling_device *cpufreq_device;
+        struct cpumask *maskPtr;
+        unsigned int cpu;
+        struct cpufreq_frequency_table *table;
+
+        mutex_lock(&cooling_cpufreq_lock);
+        list_for_each_entry(cpufreq_device, &cooling_cpufreq_list, node) {
+                if (cpufreq_device && cpufreq_device->cool_dev == cdev)
+                        break;
+        }
+        if (cpufreq_device == NULL)
+                goto return_get_max_state;
+
+        maskPtr = &cpufreq_device->allowed_cpus;
+        cpu = cpumask_any(maskPtr);
+        table = cpufreq_frequency_get_table(cpu);
+        if (!table) {
+                *state = 0;
+                ret = 0;
+                goto return_get_max_state;
+        }
+
+        while (table[i].frequency != CPUFREQ_TABLE_END) {
+                if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+                        continue;
+                i++;
+        }
+        if (i > 0) {
+                *state = --i;
+                ret = 0;
+        }
+
+return_get_max_state:
+        mutex_unlock(&cooling_cpufreq_lock);
+        return ret;
+}
+
+/**
+ * cpufreq_get_cur_state - callback function to get the current cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: fill this variable with the current cooling state.
+ */
+static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
+                                 unsigned long *state)
+{
+        int ret = -EINVAL;
+        struct cpufreq_cooling_device *cpufreq_device;
+
+        mutex_lock(&cooling_cpufreq_lock);
+        list_for_each_entry(cpufreq_device, &cooling_cpufreq_list, node) {
+                if (cpufreq_device && cpufreq_device->cool_dev == cdev) {
+                        *state = cpufreq_device->cpufreq_state;
+                        ret = 0;
+                        break;
+                }
+        }
+        mutex_unlock(&cooling_cpufreq_lock);
+
+        return ret;
+}
+
+/**
+ * cpufreq_set_cur_state - callback function to set the current cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: set this variable to the current cooling state.
+ */
+static int cpufreq_set_cur_state(struct thermal_cooling_device *cdev,
+                                 unsigned long state)
+{
+        int ret = -EINVAL;
+        struct cpufreq_cooling_device *cpufreq_device;
+
+        mutex_lock(&cooling_cpufreq_lock);
+        list_for_each_entry(cpufreq_device, &cooling_cpufreq_list, node) {
+                if (cpufreq_device && cpufreq_device->cool_dev == cdev) {
+                        ret = 0;
+                        break;
+                }
+        }
+        if (!ret)
+                ret = cpufreq_apply_cooling(cpufreq_device, state);
+
+        mutex_unlock(&cooling_cpufreq_lock);
+
+        return ret;
+}
+
+/* Bind cpufreq callbacks to thermal cooling device ops */
+static struct thermal_cooling_device_ops const cpufreq_cooling_ops = {
+        .get_max_state = cpufreq_get_max_state,
+        .get_cur_state = cpufreq_get_cur_state,
+        .set_cur_state = cpufreq_set_cur_state,
+};
+
+/* Notifier for cpufreq policy change */
+static struct notifier_block thermal_cpufreq_notifier_block = {
+        .notifier_call = cpufreq_thermal_notifier,
+};
+
+/**
+ * cpufreq_cooling_register - function to create cpufreq cooling device.
+ * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ */
+struct thermal_cooling_device *cpufreq_cooling_register(
+        struct cpumask *clip_cpus)
+{
+        struct thermal_cooling_device *cool_dev;
+        struct cpufreq_cooling_device *cpufreq_dev = NULL;
+        unsigned int cpufreq_dev_count = 0, min = 0, max = 0;
+        char dev_name[THERMAL_NAME_LENGTH];
+        int ret = 0, i;
+        struct cpufreq_policy policy;
+
+        list_for_each_entry(cpufreq_dev, &cooling_cpufreq_list, node)
+                cpufreq_dev_count++;
+
+        /*Verify that all the clip cpus have same freq_min, freq_max limit*/
+        for_each_cpu(i, clip_cpus) {
+                /*continue if cpufreq policy not found and not return error*/
+                if (!cpufreq_get_policy(&policy, i))
+                        continue;
+                if (min == 0 && max == 0) {
+                        min = policy.cpuinfo.min_freq;
+                        max = policy.cpuinfo.max_freq;
+                } else {
+                        if (min != policy.cpuinfo.min_freq ||
+                                max != policy.cpuinfo.max_freq)
+                                return ERR_PTR(-EINVAL);
+}
+        }
+        cpufreq_dev = kzalloc(sizeof(struct cpufreq_cooling_device),
+                        GFP_KERNEL);
+        if (!cpufreq_dev)
+                return ERR_PTR(-ENOMEM);
+
+        cpumask_copy(&cpufreq_dev->allowed_cpus, clip_cpus);
+
+        if (cpufreq_dev_count == 0)
+                mutex_init(&cooling_cpufreq_lock);
+
+        ret = get_idr(&cpufreq_idr, &cpufreq_dev->id);
+        if (ret) {
+                kfree(cpufreq_dev);
+                return ERR_PTR(-EINVAL);
+        }
+
+        sprintf(dev_name, "thermal-cpufreq-%d", cpufreq_dev->id);
+
+        cool_dev = thermal_cooling_device_register(dev_name, cpufreq_dev,
+                                                &cpufreq_cooling_ops);
+        if (!cool_dev) {
+                release_idr(&cpufreq_idr, cpufreq_dev->id);
+                kfree(cpufreq_dev);
+                return ERR_PTR(-EINVAL);
+        }
+        cpufreq_dev->cool_dev = cool_dev;
+        cpufreq_dev->cpufreq_state = 0;
+        mutex_lock(&cooling_cpufreq_lock);
+        list_add_tail(&cpufreq_dev->node, &cooling_cpufreq_list);
+
+        /* Register the notifier for first cpufreq cooling device */
+        if (cpufreq_dev_count == 0)
+                cpufreq_register_notifier(&thermal_cpufreq_notifier_block,
+                                                CPUFREQ_POLICY_NOTIFIER);
+
+        mutex_unlock(&cooling_cpufreq_lock);
+        return cool_dev;
+}
+EXPORT_SYMBOL(cpufreq_cooling_register);
+
+/**
+ * cpufreq_cooling_unregister - function to remove cpufreq cooling device.
+ * @cdev: thermal cooling device pointer.
+ */
+void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
+{
+        struct cpufreq_cooling_device *cpufreq_dev = NULL;
+        unsigned int cpufreq_dev_count = 0;
+
+        mutex_lock(&cooling_cpufreq_lock);
+        list_for_each_entry(cpufreq_dev, &cooling_cpufreq_list, node) {
+                if (cpufreq_dev && cpufreq_dev->cool_dev == cdev)
+                        break;
+                cpufreq_dev_count++;
+        }
+
+        if (!cpufreq_dev || cpufreq_dev->cool_dev != cdev) {
+                mutex_unlock(&cooling_cpufreq_lock);
+                return;
+        }
+
+        list_del(&cpufreq_dev->node);
+
+        /* Unregister the notifier for the last cpufreq cooling device */
+        if (cpufreq_dev_count == 1) {
+                cpufreq_unregister_notifier(&thermal_cpufreq_notifier_block,
+                                        CPUFREQ_POLICY_NOTIFIER);
+        }
+        mutex_unlock(&cooling_cpufreq_lock);
+        thermal_cooling_device_unregister(cpufreq_dev->cool_dev);
+        release_idr(&cpufreq_idr, cpufreq_dev->id);
+        if (cpufreq_dev_count == 1)
+                mutex_destroy(&cooling_cpufreq_lock);
+        kfree(cpufreq_dev);
+}
+EXPORT_SYMBOL(cpufreq_cooling_unregister);
diff --git a/drivers/thermal/exynos_thermal.c b/drivers/thermal/exynos_thermal.c
new file mode 100644
index 000000000000..6dd29e4ce36b
--- /dev/null
+++ b/drivers/thermal/exynos_thermal.c
@@ -0,0 +1,997 @@
+/*
+ * exynos_thermal.c - Samsung EXYNOS TMU (Thermal Management Unit)
+ *
+ *  Copyright (C) 2011 Samsung Electronics
+ *  Donggeun Kim <dg77.kim@samsung.com>
+ *  Amit Daniel Kachhap <amit.kachhap@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/workqueue.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/platform_data/exynos_thermal.h>
+#include <linux/thermal.h>
+#include <linux/cpufreq.h>
+#include <linux/cpu_cooling.h>
+#include <linux/of.h>
+
+#include <plat/cpu.h>
+
+/* Exynos generic registers */
+#define EXYNOS_TMU_REG_TRIMINFO         0x0
+#define EXYNOS_TMU_REG_CONTROL          0x20
+#define EXYNOS_TMU_REG_STATUS           0x28
+#define EXYNOS_TMU_REG_CURRENT_TEMP     0x40
+#define EXYNOS_TMU_REG_INTEN            0x70
+#define EXYNOS_TMU_REG_INTSTAT          0x74
+#define EXYNOS_TMU_REG_INTCLEAR         0x78
+
+#define EXYNOS_TMU_TRIM_TEMP_MASK       0xff
+#define EXYNOS_TMU_GAIN_SHIFT           8
+#define EXYNOS_TMU_REF_VOLTAGE_SHIFT    24
+#define EXYNOS_TMU_CORE_ON              3
+#define EXYNOS_TMU_CORE_OFF             2
+#define EXYNOS_TMU_DEF_CODE_TO_TEMP_OFFSET      50
+
+/* Exynos4210 specific registers */
+#define EXYNOS4210_TMU_REG_THRESHOLD_TEMP       0x44
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL0  0x50
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL1  0x54
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL2  0x58
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL3  0x5C
+#define EXYNOS4210_TMU_REG_PAST_TEMP0   0x60
+#define EXYNOS4210_TMU_REG_PAST_TEMP1   0x64
+#define EXYNOS4210_TMU_REG_PAST_TEMP2   0x68
+#define EXYNOS4210_TMU_REG_PAST_TEMP3   0x6C
+
+#define EXYNOS4210_TMU_TRIG_LEVEL0_MASK 0x1
+#define EXYNOS4210_TMU_TRIG_LEVEL1_MASK 0x10
+#define EXYNOS4210_TMU_TRIG_LEVEL2_MASK 0x100
+#define EXYNOS4210_TMU_TRIG_LEVEL3_MASK 0x1000
+#define EXYNOS4210_TMU_INTCLEAR_VAL     0x1111
+
+/* Exynos5250 and Exynos4412 specific registers */
+#define EXYNOS_TMU_TRIMINFO_CON 0x14
+#define EXYNOS_THD_TEMP_RISE            0x50
+#define EXYNOS_THD_TEMP_FALL            0x54
+#define EXYNOS_EMUL_CON         0x80
+
+#define EXYNOS_TRIMINFO_RELOAD          0x1
+#define EXYNOS_TMU_CLEAR_RISE_INT       0x111
+#define EXYNOS_TMU_CLEAR_FALL_INT       (0x111 << 16)
+#define EXYNOS_MUX_ADDR_VALUE           6
+#define EXYNOS_MUX_ADDR_SHIFT           20
+#define EXYNOS_TMU_TRIP_MODE_SHIFT      13
+
+#define EFUSE_MIN_VALUE 40
+#define EFUSE_MAX_VALUE 100
+
+/* In-kernel thermal framework related macros & definations */
+#define SENSOR_NAME_LEN 16
+#define MAX_TRIP_COUNT  8
+#define MAX_COOLING_DEVICE 4
+
+#define ACTIVE_INTERVAL 500
+#define IDLE_INTERVAL 10000
+#define MCELSIUS        1000
+
+/* CPU Zone information */
+#define PANIC_ZONE      4
+#define WARN_ZONE       3
+#define MONITOR_ZONE    2
+#define SAFE_ZONE       1
+
+#define GET_ZONE(trip) (trip + 2)
+#define GET_TRIP(zone) (zone - 2)
+
+#define EXYNOS_ZONE_COUNT       3
+
+struct exynos_tmu_data {
+        struct exynos_tmu_platform_data *pdata;
+        struct resource *mem;
+        void __iomem *base;
+        int irq;
+        enum soc_type soc;
+        struct work_struct irq_work;
+        struct mutex lock;
+        struct clk *clk;
+        u8 temp_error1, temp_error2;
+};
+
+struct  thermal_trip_point_conf {
+        int trip_val[MAX_TRIP_COUNT];
+        int trip_count;
+};
+
+struct  thermal_cooling_conf {
+        struct freq_clip_table freq_data[MAX_TRIP_COUNT];
+        int freq_clip_count;
+};
+
+struct thermal_sensor_conf {
+        char name[SENSOR_NAME_LEN];
+        int (*read_temperature)(void *data);
+        struct thermal_trip_point_conf trip_data;
+        struct thermal_cooling_conf cooling_data;
+        void *private_data;
+};
+
+struct exynos_thermal_zone {
+        enum thermal_device_mode mode;
+        struct thermal_zone_device *therm_dev;
+        struct thermal_cooling_device *cool_dev[MAX_COOLING_DEVICE];
+        unsigned int cool_dev_size;
+        struct platform_device *exynos4_dev;
+        struct thermal_sensor_conf *sensor_conf;
+        bool bind;
+};
+
+static struct exynos_thermal_zone *th_zone;
+static void exynos_unregister_thermal(void);
+static int exynos_register_thermal(struct thermal_sensor_conf *sensor_conf);
+
+/* Get mode callback functions for thermal zone */
+static int exynos_get_mode(struct thermal_zone_device *thermal,
+                        enum thermal_device_mode *mode)
+{
+        if (th_zone)
+                *mode = th_zone->mode;
+        return 0;
+}
+
+/* Set mode callback functions for thermal zone */
+static int exynos_set_mode(struct thermal_zone_device *thermal,
+                        enum thermal_device_mode mode)
+{
+        if (!th_zone->therm_dev) {
+                pr_notice("thermal zone not registered\n");
+                return 0;
+        }
+
+        mutex_lock(&th_zone->therm_dev->lock);
+
+        if (mode == THERMAL_DEVICE_ENABLED)
+                th_zone->therm_dev->polling_delay = IDLE_INTERVAL;
+        else
+                th_zone->therm_dev->polling_delay = 0;
+
+        mutex_unlock(&th_zone->therm_dev->lock);
+
+        th_zone->mode = mode;
+        thermal_zone_device_update(th_zone->therm_dev);
+        pr_info("thermal polling set for duration=%d msec\n",
+                                th_zone->therm_dev->polling_delay);
+        return 0;
+}
+
+
+/* Get trip type callback functions for thermal zone */
+static int exynos_get_trip_type(struct thermal_zone_device *thermal, int trip,
+                                 enum thermal_trip_type *type)
+{
+        switch (GET_ZONE(trip)) {
+        case MONITOR_ZONE:
+        case WARN_ZONE:
+                *type = THERMAL_TRIP_ACTIVE;
+                break;
+        case PANIC_ZONE:
+                *type = THERMAL_TRIP_CRITICAL;
+                break;
+        default:
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/* Get trip temperature callback functions for thermal zone */
+static int exynos_get_trip_temp(struct thermal_zone_device *thermal, int trip,
+                                unsigned long *temp)
+{
+        if (trip < GET_TRIP(MONITOR_ZONE) || trip > GET_TRIP(PANIC_ZONE))
+                return -EINVAL;
+
+        *temp = th_zone->sensor_conf->trip_data.trip_val[trip];
+        /* convert the temperature into millicelsius */
+        *temp = *temp * MCELSIUS;
+
+        return 0;
+}
+
+/* Get critical temperature callback functions for thermal zone */
+static int exynos_get_crit_temp(struct thermal_zone_device *thermal,
+                                unsigned long *temp)
+{
+        int ret;
+        /* Panic zone */
+        ret = exynos_get_trip_temp(thermal, GET_TRIP(PANIC_ZONE), temp);
+        return ret;
+}
+
+static int exynos_get_frequency_level(unsigned int cpu, unsigned int freq)
+{
+        int i = 0, ret = -EINVAL;
+        struct cpufreq_frequency_table *table = NULL;
+#ifdef CONFIG_CPU_FREQ
+        table = cpufreq_frequency_get_table(cpu);
+#endif
+        if (!table)
+                return ret;
+
+        while (table[i].frequency != CPUFREQ_TABLE_END) {
+                if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+                        continue;
+                if (table[i].frequency == freq)
+                        return i;
+                i++;
+        }
+        return ret;
+}
+
+/* Bind callback functions for thermal zone */
+static int exynos_bind(struct thermal_zone_device *thermal,
+                        struct thermal_cooling_device *cdev)
+{
+        int ret = 0, i, tab_size, level;
+        struct freq_clip_table *tab_ptr, *clip_data;
+        struct thermal_sensor_conf *data = th_zone->sensor_conf;
+
+        tab_ptr = (struct freq_clip_table *)data->cooling_data.freq_data;
+        tab_size = data->cooling_data.freq_clip_count;
+
+        if (tab_ptr == NULL || tab_size == 0)
+                return -EINVAL;
+
+        /* find the cooling device registered*/
+        for (i = 0; i < th_zone->cool_dev_size; i++)
+                if (cdev == th_zone->cool_dev[i])
+                        break;
+
+        /* No matching cooling device */
+        if (i == th_zone->cool_dev_size)
+                return 0;
+
+        /* Bind the thermal zone to the cpufreq cooling device */
+        for (i = 0; i < tab_size; i++) {
+                clip_data = (struct freq_clip_table *)&(tab_ptr[i]);
+                level = exynos_get_frequency_level(0, clip_data->freq_clip_max);
+                if (level < 0)
+                        return 0;
+                switch (GET_ZONE(i)) {
+                case MONITOR_ZONE:
+                case WARN_ZONE:
+                        if (thermal_zone_bind_cooling_device(thermal, i, cdev,
+                                                                level, level)) {
+                                pr_err("error binding cdev inst %d\n", i);
+                                ret = -EINVAL;
+                        }
+                        th_zone->bind = true;
+                        break;
+                default:
+                        ret = -EINVAL;
+                }
+        }
+
+        return ret;
+}
+
+/* Unbind callback functions for thermal zone */
+static int exynos_unbind(struct thermal_zone_device *thermal,
+                        struct thermal_cooling_device *cdev)
+{
+        int ret = 0, i, tab_size;
+        struct thermal_sensor_conf *data = th_zone->sensor_conf;
+
+        if (th_zone->bind == false)
+                return 0;
+
+        tab_size = data->cooling_data.freq_clip_count;
+
+        if (tab_size == 0)
+                return -EINVAL;
+
+        /* find the cooling device registered*/
+        for (i = 0; i < th_zone->cool_dev_size; i++)
+                if (cdev == th_zone->cool_dev[i])
+                        break;
+
+        /* No matching cooling device */
+        if (i == th_zone->cool_dev_size)
+                return 0;
+
+        /* Bind the thermal zone to the cpufreq cooling device */
+        for (i = 0; i < tab_size; i++) {
+                switch (GET_ZONE(i)) {
+                case MONITOR_ZONE:
+                case WARN_ZONE:
+                        if (thermal_zone_unbind_cooling_device(thermal, i,
+                                                                cdev)) {
+                                pr_err("error unbinding cdev inst=%d\n", i);
+                                ret = -EINVAL;
+                        }
+                        th_zone->bind = false;
+                        break;
+                default:
+                        ret = -EINVAL;
+                }
+        }
+        return ret;
+}
+
+/* Get temperature callback functions for thermal zone */
+static int exynos_get_temp(struct thermal_zone_device *thermal,
+                        unsigned long *temp)
+{
+        void *data;
+
+        if (!th_zone->sensor_conf) {
+                pr_info("Temperature sensor not initialised\n");
+                return -EINVAL;
+        }
+        data = th_zone->sensor_conf->private_data;
+        *temp = th_zone->sensor_conf->read_temperature(data);
+        /* convert the temperature into millicelsius */
+        *temp = *temp * MCELSIUS;
+        return 0;
+}
+
+/* Get the temperature trend */
+static int exynos_get_trend(struct thermal_zone_device *thermal,
+                        int trip, enum thermal_trend *trend)
+{
+        if (thermal->temperature >= trip)
+                *trend = THERMAL_TREND_RAISING;
+        else
+                *trend = THERMAL_TREND_DROPPING;
+
+        return 0;
+}
+/* Operation callback functions for thermal zone */
+static struct thermal_zone_device_ops const exynos_dev_ops = {
+        .bind = exynos_bind,
+        .unbind = exynos_unbind,
+        .get_temp = exynos_get_temp,
+        .get_trend = exynos_get_trend,
+        .get_mode = exynos_get_mode,
+        .set_mode = exynos_set_mode,
+        .get_trip_type = exynos_get_trip_type,
+        .get_trip_temp = exynos_get_trip_temp,
+        .get_crit_temp = exynos_get_crit_temp,
+};
+
+/*
+ * This function may be called from interrupt based temperature sensor
+ * when threshold is changed.
+ */
+static void exynos_report_trigger(void)
+{
+        unsigned int i;
+        char data[10];
+        char *envp[] = { data, NULL };
+
+        if (!th_zone || !th_zone->therm_dev)
+                return;
+        if (th_zone->bind == false) {
+                for (i = 0; i < th_zone->cool_dev_size; i++) {
+                        if (!th_zone->cool_dev[i])
+                                continue;
+                        exynos_bind(th_zone->therm_dev,
+                                        th_zone->cool_dev[i]);
+                }
+        }
+
+        thermal_zone_device_update(th_zone->therm_dev);
+
+        mutex_lock(&th_zone->therm_dev->lock);
+        /* Find the level for which trip happened */
+        for (i = 0; i < th_zone->sensor_conf->trip_data.trip_count; i++) {
+                if (th_zone->therm_dev->last_temperature <
+                        th_zone->sensor_conf->trip_data.trip_val[i] * MCELSIUS)
+                        break;
+        }
+
+        if (th_zone->mode == THERMAL_DEVICE_ENABLED) {
+                if (i > 0)
+                        th_zone->therm_dev->polling_delay = ACTIVE_INTERVAL;
+                else
+                        th_zone->therm_dev->polling_delay = IDLE_INTERVAL;
+        }
+
+        snprintf(data, sizeof(data), "%u", i);
+        kobject_uevent_env(&th_zone->therm_dev->device.kobj, KOBJ_CHANGE, envp);
+        mutex_unlock(&th_zone->therm_dev->lock);
+}
+
+/* Register with the in-kernel thermal management */
+static int exynos_register_thermal(struct thermal_sensor_conf *sensor_conf)
+{
+        int ret;
+        struct cpumask mask_val;
+
+        if (!sensor_conf || !sensor_conf->read_temperature) {
+                pr_err("Temperature sensor not initialised\n");
+                return -EINVAL;
+        }
+
+        th_zone = kzalloc(sizeof(struct exynos_thermal_zone), GFP_KERNEL);
+        if (!th_zone)
+                return -ENOMEM;
+
+        th_zone->sensor_conf = sensor_conf;
+        cpumask_set_cpu(0, &mask_val);
+        th_zone->cool_dev[0] = cpufreq_cooling_register(&mask_val);
+        if (IS_ERR(th_zone->cool_dev[0])) {
+                pr_err("Failed to register cpufreq cooling device\n");
+                ret = -EINVAL;
+                goto err_unregister;
+        }
+        th_zone->cool_dev_size++;
+
+        th_zone->therm_dev = thermal_zone_device_register(sensor_conf->name,
+                        EXYNOS_ZONE_COUNT, 0, NULL, &exynos_dev_ops, 0,
+                        IDLE_INTERVAL);
+
+        if (IS_ERR(th_zone->therm_dev)) {
+                pr_err("Failed to register thermal zone device\n");
+                ret = -EINVAL;
+                goto err_unregister;
+        }
+        th_zone->mode = THERMAL_DEVICE_ENABLED;
+
+        pr_info("Exynos: Kernel Thermal management registered\n");
+
+        return 0;
+
+err_unregister:
+        exynos_unregister_thermal();
+        return ret;
+}
+
+/* Un-Register with the in-kernel thermal management */
+static void exynos_unregister_thermal(void)
+{
+        int i;
+
+        if (!th_zone)
+                return;
+
+        if (th_zone->therm_dev)
+                thermal_zone_device_unregister(th_zone->therm_dev);
+
+        for (i = 0; i < th_zone->cool_dev_size; i++) {
+                if (th_zone->cool_dev[i])
+                        cpufreq_cooling_unregister(th_zone->cool_dev[i]);
+        }
+
+        kfree(th_zone);
+        pr_info("Exynos: Kernel Thermal management unregistered\n");
+}
+
+/*
+ * TMU treats temperature as a mapped temperature code.
+ * The temperature is converted differently depending on the calibration type.
+ */
+static int temp_to_code(struct exynos_tmu_data *data, u8 temp)
+{
+        struct exynos_tmu_platform_data *pdata = data->pdata;
+        int temp_code;
+
+        if (data->soc == SOC_ARCH_EXYNOS4210)
+                /* temp should range between 25 and 125 */
+                if (temp < 25 || temp > 125) {
+                        temp_code = -EINVAL;
+                        goto out;
+                }
+
+        switch (pdata->cal_type) {
+        case TYPE_TWO_POINT_TRIMMING:
+                temp_code = (temp - 25) *
+                    (data->temp_error2 - data->temp_error1) /
+                    (85 - 25) + data->temp_error1;
+                break;
+        case TYPE_ONE_POINT_TRIMMING:
+                temp_code = temp + data->temp_error1 - 25;
+                break;
+        default:
+                temp_code = temp + EXYNOS_TMU_DEF_CODE_TO_TEMP_OFFSET;
+                break;
+        }
+out:
+        return temp_code;
+}
+
+/*
+ * Calculate a temperature value from a temperature code.
+ * The unit of the temperature is degree Celsius.
+ */
+static int code_to_temp(struct exynos_tmu_data *data, u8 temp_code)
+{
+        struct exynos_tmu_platform_data *pdata = data->pdata;
+        int temp;
+
+        if (data->soc == SOC_ARCH_EXYNOS4210)
+                /* temp_code should range between 75 and 175 */
+                if (temp_code < 75 || temp_code > 175) {
+                        temp = -ENODATA;
+                        goto out;
+                }
+
+        switch (pdata->cal_type) {
+        case TYPE_TWO_POINT_TRIMMING:
+                temp = (temp_code - data->temp_error1) * (85 - 25) /
+                    (data->temp_error2 - data->temp_error1) + 25;
+                break;
+        case TYPE_ONE_POINT_TRIMMING:
+                temp = temp_code - data->temp_error1 + 25;
+                break;
+        default:
+                temp = temp_code - EXYNOS_TMU_DEF_CODE_TO_TEMP_OFFSET;
+                break;
+        }
+out:
+        return temp;
+}
+
+static int exynos_tmu_initialize(struct platform_device *pdev)
+{
+        struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+        struct exynos_tmu_platform_data *pdata = data->pdata;
+        unsigned int status, trim_info, rising_threshold;
+        int ret = 0, threshold_code;
+
+        mutex_lock(&data->lock);
+        clk_enable(data->clk);
+
+        status = readb(data->base + EXYNOS_TMU_REG_STATUS);
+        if (!status) {
+                ret = -EBUSY;
+                goto out;
+        }
+
+        if (data->soc == SOC_ARCH_EXYNOS) {
+                __raw_writel(EXYNOS_TRIMINFO_RELOAD,
+                                data->base + EXYNOS_TMU_TRIMINFO_CON);
+        }
+        /* Save trimming info in order to perform calibration */
+        trim_info = readl(data->base + EXYNOS_TMU_REG_TRIMINFO);
+        data->temp_error1 = trim_info & EXYNOS_TMU_TRIM_TEMP_MASK;
+        data->temp_error2 = ((trim_info >> 8) & EXYNOS_TMU_TRIM_TEMP_MASK);
+
+        if ((EFUSE_MIN_VALUE > data->temp_error1) ||
+                        (data->temp_error1 > EFUSE_MAX_VALUE) ||
+                        (data->temp_error2 != 0))
+                data->temp_error1 = pdata->efuse_value;
+
+        if (data->soc == SOC_ARCH_EXYNOS4210) {
+                /* Write temperature code for threshold */
+                threshold_code = temp_to_code(data, pdata->threshold);
+                if (threshold_code < 0) {
+                        ret = threshold_code;
+                        goto out;
+                }
+                writeb(threshold_code,
+                        data->base + EXYNOS4210_TMU_REG_THRESHOLD_TEMP);
+
+                writeb(pdata->trigger_levels[0],
+                        data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL0);
+                writeb(pdata->trigger_levels[1],
+                        data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL1);
+                writeb(pdata->trigger_levels[2],
+                        data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL2);
+                writeb(pdata->trigger_levels[3],
+                        data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL3);
+
+                writel(EXYNOS4210_TMU_INTCLEAR_VAL,
+                        data->base + EXYNOS_TMU_REG_INTCLEAR);
+        } else if (data->soc == SOC_ARCH_EXYNOS) {
+                /* Write temperature code for threshold */
+                threshold_code = temp_to_code(data, pdata->trigger_levels[0]);
+                if (threshold_code < 0) {
+                        ret = threshold_code;
+                        goto out;
+                }
+                rising_threshold = threshold_code;
+                threshold_code = temp_to_code(data, pdata->trigger_levels[1]);
+                if (threshold_code < 0) {
+                        ret = threshold_code;
+                        goto out;
+                }
+                rising_threshold |= (threshold_code << 8);
+                threshold_code = temp_to_code(data, pdata->trigger_levels[2]);
+                if (threshold_code < 0) {
+                        ret = threshold_code;
+                        goto out;
+                }
+                rising_threshold |= (threshold_code << 16);
+
+                writel(rising_threshold,
+                                data->base + EXYNOS_THD_TEMP_RISE);
+                writel(0, data->base + EXYNOS_THD_TEMP_FALL);
+
+                writel(EXYNOS_TMU_CLEAR_RISE_INT|EXYNOS_TMU_CLEAR_FALL_INT,
+                                data->base + EXYNOS_TMU_REG_INTCLEAR);
+        }
+out:
+        clk_disable(data->clk);
+        mutex_unlock(&data->lock);
+
+        return ret;
+}
+
+static void exynos_tmu_control(struct platform_device *pdev, bool on)
+{
+        struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+        struct exynos_tmu_platform_data *pdata = data->pdata;
+        unsigned int con, interrupt_en;
+
+        mutex_lock(&data->lock);
+        clk_enable(data->clk);
+
+        con = pdata->reference_voltage << EXYNOS_TMU_REF_VOLTAGE_SHIFT |
+                pdata->gain << EXYNOS_TMU_GAIN_SHIFT;
+
+        if (data->soc == SOC_ARCH_EXYNOS) {
+                con |= pdata->noise_cancel_mode << EXYNOS_TMU_TRIP_MODE_SHIFT;
+                con |= (EXYNOS_MUX_ADDR_VALUE << EXYNOS_MUX_ADDR_SHIFT);
+        }
+
+        if (on) {
+                con |= EXYNOS_TMU_CORE_ON;
+                interrupt_en = pdata->trigger_level3_en << 12 |
+                        pdata->trigger_level2_en << 8 |
+                        pdata->trigger_level1_en << 4 |
+                        pdata->trigger_level0_en;
+        } else {
+                con |= EXYNOS_TMU_CORE_OFF;
+                interrupt_en = 0; /* Disable all interrupts */
+        }
+        writel(interrupt_en, data->base + EXYNOS_TMU_REG_INTEN);
+        writel(con, data->base + EXYNOS_TMU_REG_CONTROL);
+
+        clk_disable(data->clk);
+        mutex_unlock(&data->lock);
+}
+
+static int exynos_tmu_read(struct exynos_tmu_data *data)
+{
+        u8 temp_code;
+        int temp;
+
+        mutex_lock(&data->lock);
+        clk_enable(data->clk);
+
+        temp_code = readb(data->base + EXYNOS_TMU_REG_CURRENT_TEMP);
+        temp = code_to_temp(data, temp_code);
+
+        clk_disable(data->clk);
+        mutex_unlock(&data->lock);
+
+        return temp;
+}
+
+static void exynos_tmu_work(struct work_struct *work)
+{
+        struct exynos_tmu_data *data = container_of(work,
+                        struct exynos_tmu_data, irq_work);
+
+        mutex_lock(&data->lock);
+        clk_enable(data->clk);
+
+
+        if (data->soc == SOC_ARCH_EXYNOS)
+                writel(EXYNOS_TMU_CLEAR_RISE_INT,
+                                data->base + EXYNOS_TMU_REG_INTCLEAR);
+        else
+                writel(EXYNOS4210_TMU_INTCLEAR_VAL,
+                                data->base + EXYNOS_TMU_REG_INTCLEAR);
+
+        clk_disable(data->clk);
+        mutex_unlock(&data->lock);
+        exynos_report_trigger();
+        enable_irq(data->irq);
+}
+
+static irqreturn_t exynos_tmu_irq(int irq, void *id)
+{
+        struct exynos_tmu_data *data = id;
+
+        disable_irq_nosync(irq);
+        schedule_work(&data->irq_work);
+
+        return IRQ_HANDLED;
+}
+static struct thermal_sensor_conf exynos_sensor_conf = {
+        .name                   = "exynos-therm",
+        .read_temperature       = (int (*)(void *))exynos_tmu_read,
+};
+
+#if defined(CONFIG_CPU_EXYNOS4210)
+static struct exynos_tmu_platform_data const exynos4210_default_tmu_data = {
+        .threshold = 80,
+        .trigger_levels[0] = 5,
+        .trigger_levels[1] = 20,
+        .trigger_levels[2] = 30,
+        .trigger_level0_en = 1,
+        .trigger_level1_en = 1,
+        .trigger_level2_en = 1,
+        .trigger_level3_en = 0,
+        .gain = 15,
+        .reference_voltage = 7,
+        .cal_type = TYPE_ONE_POINT_TRIMMING,
+        .freq_tab[0] = {
+                .freq_clip_max = 800 * 1000,
+                .temp_level = 85,
+        },
+        .freq_tab[1] = {
+                .freq_clip_max = 200 * 1000,
+                .temp_level = 100,
+        },
+        .freq_tab_count = 2,
+        .type = SOC_ARCH_EXYNOS4210,
+};
+#define EXYNOS4210_TMU_DRV_DATA (&exynos4210_default_tmu_data)
+#else
+#define EXYNOS4210_TMU_DRV_DATA (NULL)
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS5250) || defined(CONFIG_SOC_EXYNOS4412)
+static struct exynos_tmu_platform_data const exynos_default_tmu_data = {
+        .trigger_levels[0] = 85,
+        .trigger_levels[1] = 103,
+        .trigger_levels[2] = 110,
+        .trigger_level0_en = 1,
+        .trigger_level1_en = 1,
+        .trigger_level2_en = 1,
+        .trigger_level3_en = 0,
+        .gain = 8,
+        .reference_voltage = 16,
+        .noise_cancel_mode = 4,
+        .cal_type = TYPE_ONE_POINT_TRIMMING,
+        .efuse_value = 55,
+        .freq_tab[0] = {
+                .freq_clip_max = 800 * 1000,
+                .temp_level = 85,
+        },
+        .freq_tab[1] = {
+                .freq_clip_max = 200 * 1000,
+                .temp_level = 103,
+        },
+        .freq_tab_count = 2,
+        .type = SOC_ARCH_EXYNOS,
+};
+#define EXYNOS_TMU_DRV_DATA (&exynos_default_tmu_data)
+#else
+#define EXYNOS_TMU_DRV_DATA (NULL)
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id exynos_tmu_match[] = {
+        {
+                .compatible = "samsung,exynos4210-tmu",
+                .data = (void *)EXYNOS4210_TMU_DRV_DATA,
+        },
+        {
+                .compatible = "samsung,exynos5250-tmu",
+                .data = (void *)EXYNOS_TMU_DRV_DATA,
+        },
+        {},
+};
+MODULE_DEVICE_TABLE(of, exynos_tmu_match);
+#else
+#define  exynos_tmu_match NULL
+#endif
+
+static struct platform_device_id exynos_tmu_driver_ids[] = {
+        {
+                .name           = "exynos4210-tmu",
+                .driver_data    = (kernel_ulong_t)EXYNOS4210_TMU_DRV_DATA,
+        },
+        {
+                .name           = "exynos5250-tmu",
+                .driver_data    = (kernel_ulong_t)EXYNOS_TMU_DRV_DATA,
+        },
+        { },
+};
+MODULE_DEVICE_TABLE(platform, exynos_tmu_driver_ids);
+
+static inline struct  exynos_tmu_platform_data *exynos_get_driver_data(
+                        struct platform_device *pdev)
+{
+#ifdef CONFIG_OF
+        if (pdev->dev.of_node) {
+                const struct of_device_id *match;
+                match = of_match_node(exynos_tmu_match, pdev->dev.of_node);
+                if (!match)
+                        return NULL;
+                return (struct exynos_tmu_platform_data *) match->data;
+        }
+#endif
+        return (struct exynos_tmu_platform_data *)
+                        platform_get_device_id(pdev)->driver_data;
+}
+static int __devinit exynos_tmu_probe(struct platform_device *pdev)
+{
+        struct exynos_tmu_data *data;
+        struct exynos_tmu_platform_data *pdata = pdev->dev.platform_data;
+        int ret, i;
+
+        if (!pdata)
+                pdata = exynos_get_driver_data(pdev);
+
+        if (!pdata) {
+                dev_err(&pdev->dev, "No platform init data supplied.\n");
+                return -ENODEV;
+        }
+        data = devm_kzalloc(&pdev->dev, sizeof(struct exynos_tmu_data),
+                                        GFP_KERNEL);
+        if (!data) {
+                dev_err(&pdev->dev, "Failed to allocate driver structure\n");
+                return -ENOMEM;
+        }
+
+        data->irq = platform_get_irq(pdev, 0);
+        if (data->irq < 0) {
+                dev_err(&pdev->dev, "Failed to get platform irq\n");
+                return data->irq;
+        }
+
+        INIT_WORK(&data->irq_work, exynos_tmu_work);
+
+        data->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!data->mem) {
+                dev_err(&pdev->dev, "Failed to get platform resource\n");
+                return -ENOENT;
+        }
+
+        data->base = devm_request_and_ioremap(&pdev->dev, data->mem);
+        if (!data->base) {
+                dev_err(&pdev->dev, "Failed to ioremap memory\n");
+                return -ENODEV;
+        }
+
+        ret = devm_request_irq(&pdev->dev, data->irq, exynos_tmu_irq,
+                IRQF_TRIGGER_RISING, "exynos-tmu", data);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to request irq: %d\n", data->irq);
+                return ret;
+        }
+
+        data->clk = clk_get(NULL, "tmu_apbif");
+        if (IS_ERR(data->clk)) {
+                dev_err(&pdev->dev, "Failed to get clock\n");
+                return  PTR_ERR(data->clk);
+        }
+
+        if (pdata->type == SOC_ARCH_EXYNOS ||
+                                pdata->type == SOC_ARCH_EXYNOS4210)
+                data->soc = pdata->type;
+        else {
+                ret = -EINVAL;
+                dev_err(&pdev->dev, "Platform not supported\n");
+                goto err_clk;
+        }
+
+        data->pdata = pdata;
+        platform_set_drvdata(pdev, data);
+        mutex_init(&data->lock);
+
+        ret = exynos_tmu_initialize(pdev);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to initialize TMU\n");
+                goto err_clk;
+        }
+
+        exynos_tmu_control(pdev, true);
+
+        /* Register the sensor with thermal management interface */
+        (&exynos_sensor_conf)->private_data = data;
+        exynos_sensor_conf.trip_data.trip_count = pdata->trigger_level0_en +
+                        pdata->trigger_level1_en + pdata->trigger_level2_en +
+                        pdata->trigger_level3_en;
+
+        for (i = 0; i < exynos_sensor_conf.trip_data.trip_count; i++)
+                exynos_sensor_conf.trip_data.trip_val[i] =
+                        pdata->threshold + pdata->trigger_levels[i];
+
+        exynos_sensor_conf.cooling_data.freq_clip_count =
+                                                pdata->freq_tab_count;
+        for (i = 0; i < pdata->freq_tab_count; i++) {
+                exynos_sensor_conf.cooling_data.freq_data[i].freq_clip_max =
+                                        pdata->freq_tab[i].freq_clip_max;
+                exynos_sensor_conf.cooling_data.freq_data[i].temp_level =
+                                        pdata->freq_tab[i].temp_level;
+        }
+
+        ret = exynos_register_thermal(&exynos_sensor_conf);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to register thermal interface\n");
+                goto err_clk;
+        }
+        return 0;
+err_clk:
+        platform_set_drvdata(pdev, NULL);
+        clk_put(data->clk);
+        return ret;
+}
+
+static int __devexit exynos_tmu_remove(struct platform_device *pdev)
+{
+        struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+
+        exynos_tmu_control(pdev, false);
+
+        exynos_unregister_thermal();
+
+        clk_put(data->clk);
+
+        platform_set_drvdata(pdev, NULL);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int exynos_tmu_suspend(struct device *dev)
+{
+        exynos_tmu_control(to_platform_device(dev), false);
+
+        return 0;
+}
+
+static int exynos_tmu_resume(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        exynos_tmu_initialize(pdev);
+        exynos_tmu_control(pdev, true);
+
+        return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(exynos_tmu_pm,
+                         exynos_tmu_suspend, exynos_tmu_resume);
+#define EXYNOS_TMU_PM   (&exynos_tmu_pm)
+#else
+#define EXYNOS_TMU_PM   NULL
+#endif
+
+static struct platform_driver exynos_tmu_driver = {
+        .driver = {
+                .name   = "exynos-tmu",
+                .owner  = THIS_MODULE,
+                .pm     = EXYNOS_TMU_PM,
+                .of_match_table = exynos_tmu_match,
+        },
+        .probe = exynos_tmu_probe,
+        .remove = __devexit_p(exynos_tmu_remove),
+        .id_table = exynos_tmu_driver_ids,
+};
+
+module_platform_driver(exynos_tmu_driver);
+
+MODULE_DESCRIPTION("EXYNOS TMU Driver");
+MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:exynos-tmu");
diff --git a/drivers/thermal/rcar_thermal.c b/drivers/thermal/rcar_thermal.c
new file mode 100644
index 000000000000..f7a1b574a304
--- /dev/null
+++ b/drivers/thermal/rcar_thermal.c
@@ -0,0 +1,260 @@
+/*
+ *  R-Car THS/TSC thermal sensor driver
+ *
+ * Copyright (C) 2012 Renesas Solutions Corp.
+ * Kuninori Morimoto <kuninori.morimoto.gx@renesas.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; version 2 of the License.
+ *
+ *  This program is distributed in the hope that it will be useful, but
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/thermal.h>
+
+#define THSCR   0x2c
+#define THSSR   0x30
+
+/* THSCR */
+#define CPTAP   0xf
+
+/* THSSR */
+#define CTEMP   0x3f
+
+
+struct rcar_thermal_priv {
+        void __iomem *base;
+        struct device *dev;
+        spinlock_t lock;
+        u32 comp;
+};
+
+/*
+ *              basic functions
+ */
+static u32 rcar_thermal_read(struct rcar_thermal_priv *priv, u32 reg)
+{
+        unsigned long flags;
+        u32 ret;
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        ret = ioread32(priv->base + reg);
+
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        return ret;
+}
+
+#if 0 /* no user at this point */
+static void rcar_thermal_write(struct rcar_thermal_priv *priv,
+                               u32 reg, u32 data)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        iowrite32(data, priv->base + reg);
+
+        spin_unlock_irqrestore(&priv->lock, flags);
+}
+#endif
+
+static void rcar_thermal_bset(struct rcar_thermal_priv *priv, u32 reg,
+                              u32 mask, u32 data)
+{
+        unsigned long flags;
+        u32 val;
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        val = ioread32(priv->base + reg);
+        val &= ~mask;
+        val |= (data & mask);
+        iowrite32(val, priv->base + reg);
+
+        spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/*
+ *              zone device functions
+ */
+static int rcar_thermal_get_temp(struct thermal_zone_device *zone,
+                           unsigned long *temp)
+{
+        struct rcar_thermal_priv *priv = zone->devdata;
+        int val, min, max, tmp;
+
+        tmp = -200; /* default */
+        while (1) {
+                if (priv->comp < 1 || priv->comp > 12) {
+                        dev_err(priv->dev,
+                                "THSSR invalid data (%d)\n", priv->comp);
+                        priv->comp = 4; /* for next thermal */
+                        return -EINVAL;
+                }
+
+                /*
+                 * THS comparator offset and the reference temperature
+                 *
+                 * Comparator   | reference     | Temperature field
+                 * offset       | temperature   | measurement
+                 *              | (degrees C)   | (degrees C)
+                 * -------------+---------------+-------------------
+                 *  1           |  -45          |  -45 to  -30
+                 *  2           |  -30          |  -30 to  -15
+                 *  3           |  -15          |  -15 to    0
+                 *  4           |    0          |    0 to  +15
+                 *  5           |  +15          |  +15 to  +30
+                 *  6           |  +30          |  +30 to  +45
+                 *  7           |  +45          |  +45 to  +60
+                 *  8           |  +60          |  +60 to  +75
+                 *  9           |  +75          |  +75 to  +90
+                 * 10           |  +90          |  +90 to +105
+                 * 11           | +105          | +105 to +120
+                 * 12           | +120          | +120 to +135
+                 */
+
+                /* calculate thermal limitation */
+                min = (priv->comp * 15) - 60;
+                max = min + 15;
+
+                /*
+                 * we need to wait 300us after changing comparator offset
+                 * to get stable temperature.
+                 * see "Usage Notes" on datasheet
+                 */
+                rcar_thermal_bset(priv, THSCR, CPTAP, priv->comp);
+                udelay(300);
+
+                /* calculate current temperature */
+                val = rcar_thermal_read(priv, THSSR) & CTEMP;
+                val = (val * 5) - 65;
+
+                dev_dbg(priv->dev, "comp/min/max/val = %d/%d/%d/%d\n",
+                        priv->comp, min, max, val);
+
+                /*
+                 * If val is same as min/max, then,
+                 * it should try again on next comparator.
+                 * But the val might be correct temperature.
+                 * Keep it on "tmp" and compare with next val.
+                 */
+                if (tmp == val)
+                        break;
+
+                if (val <= min) {
+                        tmp = min;
+                        priv->comp--; /* try again */
+                } else if (val >= max) {
+                        tmp = max;
+                        priv->comp++; /* try again */
+                } else {
+                        tmp = val;
+                        break;
+                }
+        }
+
+        *temp = tmp;
+        return 0;
+}
+
+static struct thermal_zone_device_ops rcar_thermal_zone_ops = {
+        .get_temp = rcar_thermal_get_temp,
+};
+
+/*
+ *              platform functions
+ */
+static int rcar_thermal_probe(struct platform_device *pdev)
+{
+        struct thermal_zone_device *zone;
+        struct rcar_thermal_priv *priv;
+        struct resource *res;
+        int ret;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "Could not get platform resource\n");
+                return -ENODEV;
+        }
+
+        priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+        if (!priv) {
+                dev_err(&pdev->dev, "Could not allocate priv\n");
+                return -ENOMEM;
+        }
+
+        priv->comp = 4; /* basic setup */
+        priv->dev = &pdev->dev;
+        spin_lock_init(&priv->lock);
+        priv->base = devm_ioremap_nocache(&pdev->dev,
+                                          res->start, resource_size(res));
+        if (!priv->base) {
+                dev_err(&pdev->dev, "Unable to ioremap thermal register\n");
+                ret = -ENOMEM;
+                goto error_free_priv;
+        }
+
+        zone = thermal_zone_device_register("rcar_thermal", 0, 0, priv,
+                                            &rcar_thermal_zone_ops, 0, 0);
+        if (IS_ERR(zone)) {
+                dev_err(&pdev->dev, "thermal zone device is NULL\n");
+                ret = PTR_ERR(zone);
+                goto error_iounmap;
+        }
+
+        platform_set_drvdata(pdev, zone);
+
+        dev_info(&pdev->dev, "proved\n");
+
+        return 0;
+
+error_iounmap:
+        devm_iounmap(&pdev->dev, priv->base);
+error_free_priv:
+        devm_kfree(&pdev->dev, priv);
+
+        return ret;
+}
+
+static int rcar_thermal_remove(struct platform_device *pdev)
+{
+        struct thermal_zone_device *zone = platform_get_drvdata(pdev);
+        struct rcar_thermal_priv *priv = zone->devdata;
+
+        thermal_zone_device_unregister(zone);
+        platform_set_drvdata(pdev, NULL);
+
+        devm_iounmap(&pdev->dev, priv->base);
+        devm_kfree(&pdev->dev, priv);
+
+        return 0;
+}
+
+static struct platform_driver rcar_thermal_driver = {
+        .driver = {
+                .name   = "rcar_thermal",
+        },
+        .probe          = rcar_thermal_probe,
+        .remove         = rcar_thermal_remove,
+};
+module_platform_driver(rcar_thermal_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("R-Car THS/TSC thermal sensor driver");
+MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
diff --git a/drivers/thermal/spear_thermal.c b/drivers/thermal/spear_thermal.c
index 5f8ee39f2000..9bc969261d01 100644
--- a/drivers/thermal/spear_thermal.c
+++ b/drivers/thermal/spear_thermal.c
@@ -147,7 +147,7 @@ static int spear_thermal_probe(struct platform_device *pdev)
         writel_relaxed(stdev->flags, stdev->thermal_base);
 
         spear_thermal = thermal_zone_device_register("spear_thermal", 0, 0,
-                                stdev, &ops, 0, 0, 0, 0);
+                                stdev, &ops, 0, 0);
         if (IS_ERR(spear_thermal)) {
                 dev_err(&pdev->dev, "thermal zone device is NULL\n");
                 ret = PTR_ERR(spear_thermal);
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_sys.c
index efd81bb25e01..9ee42ca4d289 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_sys.c
@@ -41,15 +41,25 @@ MODULE_AUTHOR("Zhang Rui");
 MODULE_DESCRIPTION("Generic thermal management sysfs support");
 MODULE_LICENSE("GPL");
 
-struct thermal_cooling_device_instance {
+#define THERMAL_NO_TARGET -1UL
+/*
+ * This structure is used to describe the behavior of
+ * a certain cooling device on a certain trip point
+ * in a certain thermal zone
+ */
+struct thermal_instance {
         int id;
         char name[THERMAL_NAME_LENGTH];
         struct thermal_zone_device *tz;
         struct thermal_cooling_device *cdev;
         int trip;
+        unsigned long upper;    /* Highest cooling state for this trip point */
+        unsigned long lower;    /* Lowest cooling state for this trip point */
+        unsigned long target;   /* expected cooling state */
         char attr_name[THERMAL_NAME_LENGTH];
         struct device_attribute attr;
-        struct list_head node;
+        struct list_head tz_node; /* node in tz->thermal_instances */
+        struct list_head cdev_node; /* node in cdev->thermal_instances */
 };
 
 static DEFINE_IDR(thermal_tz_idr);
@@ -308,8 +318,9 @@ passive_store(struct device *dev, struct device_attribute *attr,
                         if (!strncmp("Processor", cdev->type,
                                      sizeof("Processor")))
                                 thermal_zone_bind_cooling_device(tz,
-                                                                 THERMAL_TRIPS_NONE,
-                                                                 cdev);
+                                                THERMAL_TRIPS_NONE, cdev,
+                                                THERMAL_NO_LIMIT,
+                                                THERMAL_NO_LIMIT);
                 }
                 mutex_unlock(&thermal_list_lock);
                 if (!tz->passive_delay)
@@ -327,9 +338,6 @@ passive_store(struct device *dev, struct device_attribute *attr,
                 tz->passive_delay = 0;
         }
 
-        tz->tc1 = 1;
-        tz->tc2 = 1;
-
         tz->forced_passive = state;
 
         thermal_zone_device_update(tz);
@@ -425,10 +433,10 @@ static ssize_t
 thermal_cooling_device_trip_point_show(struct device *dev,
                                        struct device_attribute *attr, char *buf)
 {
-        struct thermal_cooling_device_instance *instance;
+        struct thermal_instance *instance;
 
         instance =
-            container_of(attr, struct thermal_cooling_device_instance, attr);
+            container_of(attr, struct thermal_instance, attr);
 
         if (instance->trip == THERMAL_TRIPS_NONE)
                 return sprintf(buf, "-1\n");
@@ -590,7 +598,7 @@ thermal_add_hwmon_sysfs(struct thermal_zone_device *tz)
         temp->tz = tz;
         hwmon->count++;
 
-        snprintf(temp->temp_input.name, THERMAL_NAME_LENGTH,
+        snprintf(temp->temp_input.name, sizeof(temp->temp_input.name),
                  "temp%d_input", hwmon->count);
         temp->temp_input.attr.attr.name = temp->temp_input.name;
         temp->temp_input.attr.attr.mode = 0444;
@@ -603,7 +611,8 @@ thermal_add_hwmon_sysfs(struct thermal_zone_device *tz)
         if (tz->ops->get_crit_temp) {
                 unsigned long temperature;
                 if (!tz->ops->get_crit_temp(tz, &temperature)) {
-                        snprintf(temp->temp_crit.name, THERMAL_NAME_LENGTH,
+                        snprintf(temp->temp_crit.name,
+                                 sizeof(temp->temp_crit.name),
                                 "temp%d_crit", hwmon->count);
                         temp->temp_crit.attr.attr.name = temp->temp_crit.name;
                         temp->temp_crit.attr.attr.mode = 0444;
@@ -704,74 +713,6 @@ static void thermal_zone_device_set_polling(struct thermal_zone_device *tz,
                 cancel_delayed_work(&tz->poll_queue);
 }
 
-static void thermal_zone_device_passive(struct thermal_zone_device *tz,
-                                        int temp, int trip_temp, int trip)
-{
-        int trend = 0;
-        struct thermal_cooling_device_instance *instance;
-        struct thermal_cooling_device *cdev;
-        long state, max_state;
-
-        /*
-         * Above Trip?
-         * -----------
-         * Calculate the thermal trend (using the passive cooling equation)
-         * and modify the performance limit for all passive cooling devices
-         * accordingly.  Note that we assume symmetry.
-         */
-        if (temp >= trip_temp) {
-                tz->passive = true;
-
-                trend = (tz->tc1 * (temp - tz->last_temperature)) +
-                        (tz->tc2 * (temp - trip_temp));
-
-                /* Heating up? */
-                if (trend > 0) {
-                        list_for_each_entry(instance, &tz->cooling_devices,
-                                            node) {
-                                if (instance->trip != trip)
-                                        continue;
-                                cdev = instance->cdev;
-                                cdev->ops->get_cur_state(cdev, &state);
-                                cdev->ops->get_max_state(cdev, &max_state);
-                                if (state++ < max_state)
-                                        cdev->ops->set_cur_state(cdev, state);
-                        }
-                } else if (trend < 0) { /* Cooling off? */
-                        list_for_each_entry(instance, &tz->cooling_devices,
-                                            node) {
-                                if (instance->trip != trip)
-                                        continue;
-                                cdev = instance->cdev;
-                                cdev->ops->get_cur_state(cdev, &state);
-                                cdev->ops->get_max_state(cdev, &max_state);
-                                if (state > 0)
-                                        cdev->ops->set_cur_state(cdev, --state);
-                        }
-                }
-                return;
-        }
-
-        /*
-         * Below Trip?
-         * -----------
-         * Implement passive cooling hysteresis to slowly increase performance
-         * and avoid thrashing around the passive trip point.  Note that we
-         * assume symmetry.
-         */
-        list_for_each_entry(instance, &tz->cooling_devices, node) {
-                if (instance->trip != trip)
-                        continue;
-                cdev = instance->cdev;
-                cdev->ops->get_cur_state(cdev, &state);
-                cdev->ops->get_max_state(cdev, &max_state);
-                if (state > 0)
-                        cdev->ops->set_cur_state(cdev, --state);
-                if (state == 0)
-                        tz->passive = false;
-        }
-}
-
 static void thermal_zone_device_check(struct work_struct *work)
 {
         struct thermal_zone_device *tz = container_of(work, struct
@@ -791,12 +732,14 @@ static void thermal_zone_device_check(struct work_struct *work)
  */
 int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
                                      int trip,
-                                     struct thermal_cooling_device *cdev)
+                                     struct thermal_cooling_device *cdev,
+                                     unsigned long upper, unsigned long lower)
 {
-        struct thermal_cooling_device_instance *dev;
-        struct thermal_cooling_device_instance *pos;
+        struct thermal_instance *dev;
+        struct thermal_instance *pos;
         struct thermal_zone_device *pos1;
         struct thermal_cooling_device *pos2;
+        unsigned long max_state;
         int result;
 
         if (trip >= tz->trips || (trip < 0 && trip != THERMAL_TRIPS_NONE))
@@ -814,13 +757,26 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
         if (tz != pos1 || cdev != pos2)
                 return -EINVAL;
 
+        cdev->ops->get_max_state(cdev, &max_state);
+
+        /* lower default 0, upper default max_state */
+        lower = lower == THERMAL_NO_LIMIT ? 0 : lower;
+        upper = upper == THERMAL_NO_LIMIT ? max_state : upper;
+
+        if (lower > upper || upper > max_state)
+                return -EINVAL;
+
         dev =
-            kzalloc(sizeof(struct thermal_cooling_device_instance), GFP_KERNEL);
+            kzalloc(sizeof(struct thermal_instance), GFP_KERNEL);
         if (!dev)
                 return -ENOMEM;
         dev->tz = tz;
         dev->cdev = cdev;
         dev->trip = trip;
+        dev->upper = upper;
+        dev->lower = lower;
+        dev->target = THERMAL_NO_TARGET;
+
         result = get_idr(&tz->idr, &tz->lock, &dev->id);
         if (result)
                 goto free_mem;
@@ -841,13 +797,17 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
                 goto remove_symbol_link;
 
         mutex_lock(&tz->lock);
-        list_for_each_entry(pos, &tz->cooling_devices, node)
+        mutex_lock(&cdev->lock);
+        list_for_each_entry(pos, &tz->thermal_instances, tz_node)
             if (pos->tz == tz && pos->trip == trip && pos->cdev == cdev) {
                 result = -EEXIST;
                 break;
         }
-        if (!result)
-                list_add_tail(&dev->node, &tz->cooling_devices);
+        if (!result) {
+                list_add_tail(&dev->tz_node, &tz->thermal_instances);
+                list_add_tail(&dev->cdev_node, &cdev->thermal_instances);
+        }
+        mutex_unlock(&cdev->lock);
         mutex_unlock(&tz->lock);
 
         if (!result)
@@ -877,16 +837,20 @@ int thermal_zone_unbind_cooling_device(struct thermal_zone_device *tz,
                                        int trip,
                                        struct thermal_cooling_device *cdev)
 {
-        struct thermal_cooling_device_instance *pos, *next;
+        struct thermal_instance *pos, *next;
 
         mutex_lock(&tz->lock);
-        list_for_each_entry_safe(pos, next, &tz->cooling_devices, node) {
+        mutex_lock(&cdev->lock);
+        list_for_each_entry_safe(pos, next, &tz->thermal_instances, tz_node) {
                 if (pos->tz == tz && pos->trip == trip && pos->cdev == cdev) {
-                        list_del(&pos->node);
+                        list_del(&pos->tz_node);
+                        list_del(&pos->cdev_node);
+                        mutex_unlock(&cdev->lock);
                         mutex_unlock(&tz->lock);
                         goto unbind;
                 }
         }
+        mutex_unlock(&cdev->lock);
         mutex_unlock(&tz->lock);
 
         return -ENODEV;
@@ -934,7 +898,7 @@ thermal_cooling_device_register(char *type, void *devdata,
         struct thermal_zone_device *pos;
         int result;
 
-        if (strlen(type) >= THERMAL_NAME_LENGTH)
+        if (type && strlen(type) >= THERMAL_NAME_LENGTH)
                 return ERR_PTR(-EINVAL);
 
         if (!ops || !ops->get_max_state || !ops->get_cur_state ||
@@ -951,8 +915,11 @@ thermal_cooling_device_register(char *type, void *devdata,
                 return ERR_PTR(result);
         }
 
-        strcpy(cdev->type, type);
+        strcpy(cdev->type, type ? : "");
+        mutex_init(&cdev->lock);
+        INIT_LIST_HEAD(&cdev->thermal_instances);
         cdev->ops = ops;
+        cdev->updated = true;
         cdev->device.class = &thermal_class;
         cdev->devdata = devdata;
         dev_set_name(&cdev->device, "cooling_device%d", cdev->id);
@@ -1044,6 +1011,136 @@ void thermal_cooling_device_unregister(struct
 }
 EXPORT_SYMBOL(thermal_cooling_device_unregister);
 
+static void thermal_cdev_do_update(struct thermal_cooling_device *cdev)
+{
+        struct thermal_instance *instance;
+        unsigned long target = 0;
+
+        /* cooling device is updated*/
+        if (cdev->updated)
+                return;
+
+        mutex_lock(&cdev->lock);
+        /* Make sure cdev enters the deepest cooling state */
+        list_for_each_entry(instance, &cdev->thermal_instances, cdev_node) {
+                if (instance->target == THERMAL_NO_TARGET)
+                        continue;
+                if (instance->target > target)
+                        target = instance->target;
+        }
+        mutex_unlock(&cdev->lock);
+        cdev->ops->set_cur_state(cdev, target);
+        cdev->updated = true;
+}
+
+static void thermal_zone_do_update(struct thermal_zone_device *tz)
+{
+        struct thermal_instance *instance;
+
+        list_for_each_entry(instance, &tz->thermal_instances, tz_node)
+                thermal_cdev_do_update(instance->cdev);
+}
+
+/*
+ * Cooling algorithm for both active and passive cooling
+ *
+ * 1. if the temperature is higher than a trip point,
+ *    a. if the trend is THERMAL_TREND_RAISING, use higher cooling
+ *       state for this trip point
+ *    b. if the trend is THERMAL_TREND_DROPPING, use lower cooling
+ *       state for this trip point
+ *
+ * 2. if the temperature is lower than a trip point, use lower
+ *    cooling state for this trip point
+ *
+ * Note that this behaves the same as the previous passive cooling
+ * algorithm.
+ */
+
+static void thermal_zone_trip_update(struct thermal_zone_device *tz,
+                                     int trip, long temp)
+{
+        struct thermal_instance *instance;
+        struct thermal_cooling_device *cdev = NULL;
+        unsigned long cur_state, max_state;
+        long trip_temp;
+        enum thermal_trip_type trip_type;
+        enum thermal_trend trend;
+
+        if (trip == THERMAL_TRIPS_NONE) {
+                trip_temp = tz->forced_passive;
+                trip_type = THERMAL_TRIPS_NONE;
+        } else {
+                tz->ops->get_trip_temp(tz, trip, &trip_temp);
+                tz->ops->get_trip_type(tz, trip, &trip_type);
+        }
+
+        if (!tz->ops->get_trend || tz->ops->get_trend(tz, trip, &trend)) {
+                /*
+                 * compare the current temperature and previous temperature
+                 * to get the thermal trend, if no special requirement
+                 */
+                if (tz->temperature > tz->last_temperature)
+                        trend = THERMAL_TREND_RAISING;
+                else if (tz->temperature < tz->last_temperature)
+                        trend = THERMAL_TREND_DROPPING;
+                else
+                        trend = THERMAL_TREND_STABLE;
+        }
+
+        if (temp >= trip_temp) {
+                list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+                        if (instance->trip != trip)
+                                continue;
+
+                        cdev = instance->cdev;
+
+                        cdev->ops->get_cur_state(cdev, &cur_state);
+                        cdev->ops->get_max_state(cdev, &max_state);
+
+                        if (trend == THERMAL_TREND_RAISING) {
+                                cur_state = cur_state < instance->upper ?
+                                            (cur_state + 1) : instance->upper;
+                        } else if (trend == THERMAL_TREND_DROPPING) {
+                                cur_state = cur_state > instance->lower ?
+                                    (cur_state - 1) : instance->lower;
+                        }
+
+                        /* activate a passive thermal instance */
+                        if ((trip_type == THERMAL_TRIP_PASSIVE ||
+                             trip_type == THERMAL_TRIPS_NONE) &&
+                             instance->target == THERMAL_NO_TARGET)
+                                tz->passive++;
+
+                        instance->target = cur_state;
+                        cdev->updated = false; /* cooling device needs update */
+                }
+        } else {        /* below trip */
+                list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+                        if (instance->trip != trip)
+                                continue;
+
+                        /* Do not use the inactive thermal instance */
+                        if (instance->target == THERMAL_NO_TARGET)
+                                continue;
+                        cdev = instance->cdev;
+                        cdev->ops->get_cur_state(cdev, &cur_state);
+
+                        cur_state = cur_state > instance->lower ?
+                                    (cur_state - 1) : THERMAL_NO_TARGET;
+
+                        /* deactivate a passive thermal instance */
+                        if ((trip_type == THERMAL_TRIP_PASSIVE ||
+                             trip_type == THERMAL_TRIPS_NONE) &&
+                             cur_state == THERMAL_NO_TARGET)
+                                tz->passive--;
+                        instance->target = cur_state;
+                        cdev->updated = false; /* cooling device needs update */
+                }
+        }
+
+        return;
+}
 /**
  * thermal_zone_device_update - force an update of a thermal zone's state
  * @ttz:        the thermal zone to update
@@ -1054,8 +1151,6 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
         int count, ret = 0;
         long temp, trip_temp;
         enum thermal_trip_type trip_type;
-        struct thermal_cooling_device_instance *instance;
-        struct thermal_cooling_device *cdev;
 
         mutex_lock(&tz->lock);
 
@@ -1065,6 +1160,9 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
                 goto leave;
         }
 
+        tz->last_temperature = tz->temperature;
+        tz->temperature = temp;
+
         for (count = 0; count < tz->trips; count++) {
                 tz->ops->get_trip_type(tz, count, &trip_type);
                 tz->ops->get_trip_temp(tz, count, &trip_temp);
@@ -1088,32 +1186,18 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
                                         tz->ops->notify(tz, count, trip_type);
                         break;
                 case THERMAL_TRIP_ACTIVE:
-                        list_for_each_entry(instance, &tz->cooling_devices,
-                                            node) {
-                                if (instance->trip != count)
-                                        continue;
-
-                                cdev = instance->cdev;
-
-                                if (temp >= trip_temp)
-                                        cdev->ops->set_cur_state(cdev, 1);
-                                else
-                                        cdev->ops->set_cur_state(cdev, 0);
-                        }
+                        thermal_zone_trip_update(tz, count, temp);
                         break;
                 case THERMAL_TRIP_PASSIVE:
                         if (temp >= trip_temp || tz->passive)
-                                thermal_zone_device_passive(tz, temp,
-                                                            trip_temp, count);
+                                thermal_zone_trip_update(tz, count, temp);
                         break;
                 }
         }
 
         if (tz->forced_passive)
-                thermal_zone_device_passive(tz, temp, tz->forced_passive,
-                                            THERMAL_TRIPS_NONE);
-
-        tz->last_temperature = temp;
+                thermal_zone_trip_update(tz, THERMAL_TRIPS_NONE, temp);
+        thermal_zone_do_update(tz);
 
 leave:
         if (tz->passive)
@@ -1236,8 +1320,6 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
  * @mask:       a bit string indicating the writeablility of trip points
  * @devdata:    private device data
  * @ops:        standard thermal zone device callbacks
- * @tc1:        thermal coefficient 1 for passive calculations
- * @tc2:        thermal coefficient 2 for passive calculations
  * @passive_delay: number of milliseconds to wait between polls when
  *                 performing passive cooling
  * @polling_delay: number of milliseconds to wait between polls when checking
@@ -1245,13 +1327,12 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
  *                 driven systems)
  *
  * thermal_zone_device_unregister() must be called when the device is no
- * longer needed. The passive cooling formula uses tc1 and tc2 as described in
- * section 11.1.5.1 of the ACPI specification 3.0.
+ * longer needed. The passive cooling depends on the .get_trend() return value.
  */
 struct thermal_zone_device *thermal_zone_device_register(const char *type,
         int trips, int mask, void *devdata,
         const struct thermal_zone_device_ops *ops,
-        int tc1, int tc2, int passive_delay, int polling_delay)
+        int passive_delay, int polling_delay)
 {
         struct thermal_zone_device *tz;
         struct thermal_cooling_device *pos;
@@ -1260,7 +1341,7 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
         int count;
         int passive = 0;
 
-        if (strlen(type) >= THERMAL_NAME_LENGTH)
+        if (type && strlen(type) >= THERMAL_NAME_LENGTH)
                 return ERR_PTR(-EINVAL);
 
         if (trips > THERMAL_MAX_TRIPS || trips < 0 || mask >> trips)
@@ -1273,7 +1354,7 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
         if (!tz)
                 return ERR_PTR(-ENOMEM);
 
-        INIT_LIST_HEAD(&tz->cooling_devices);
+        INIT_LIST_HEAD(&tz->thermal_instances);
         idr_init(&tz->idr);
         mutex_init(&tz->lock);
         result = get_idr(&thermal_tz_idr, &thermal_idr_lock, &tz->id);
@@ -1282,13 +1363,11 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
                 return ERR_PTR(result);
         }
 
-        strcpy(tz->type, type);
+        strcpy(tz->type, type ? : "");
         tz->ops = ops;
         tz->device.class = &thermal_class;
         tz->devdata = devdata;
         tz->trips = trips;
-        tz->tc1 = tc1;
-        tz->tc2 = tc2;
         tz->passive_delay = passive_delay;
         tz->polling_delay = polling_delay;
 
diff --git a/drivers/tty/hvc/hvc_xen.c b/drivers/tty/hvc/hvc_xen.c
index 2944ff88fdc0..f4abfe238f98 100644
--- a/drivers/tty/hvc/hvc_xen.c
+++ b/drivers/tty/hvc/hvc_xen.c
@@ -478,7 +478,6 @@ static void xencons_backend_changed(struct xenbus_device *dev,
         case XenbusStateInitialising:
         case XenbusStateInitialised:
         case XenbusStateUnknown:
-        case XenbusStateClosed:
                 break;
 
         case XenbusStateInitWait:
@@ -488,6 +487,10 @@ static void xencons_backend_changed(struct xenbus_device *dev,
                 xenbus_switch_state(dev, XenbusStateConnected);
                 break;
 
+        case XenbusStateClosed:
+                if (dev->state == XenbusStateClosed)
+                        break;
+                /* Missed the backend's CLOSING state -- fallthrough */
         case XenbusStateClosing:
                 xenbus_frontend_closed(dev);
                 break;
diff --git a/drivers/tty/serial/8250/8250_hp300.c b/drivers/tty/serial/8250/8250_hp300.c
index 8f1dd2cc00a8..f3d0edf46644 100644
--- a/drivers/tty/serial/8250/8250_hp300.c
+++ b/drivers/tty/serial/8250/8250_hp300.c
@@ -162,7 +162,7 @@ int __init hp300_setup_serial_console(void)
 static int __devinit hpdca_init_one(struct dio_dev *d,
                                 const struct dio_device_id *ent)
 {
-        struct uart_port port;
+        struct uart_8250_port uart;
         int line;
 
 #ifdef CONFIG_SERIAL_8250_CONSOLE
@@ -174,19 +174,19 @@ static int __devinit hpdca_init_one(struct dio_dev *d,
         memset(&uart, 0, sizeof(uart));
 
         /* Memory mapped I/O */
-        port.iotype = UPIO_MEM;
-        port.flags = UPF_SKIP_TEST | UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF;
-        port.irq = d->ipl;
-        port.uartclk = HPDCA_BAUD_BASE * 16;
-        port.mapbase = (d->resource.start + UART_OFFSET);
-        port.membase = (char *)(port.mapbase + DIO_VIRADDRBASE);
-        port.regshift = 1;
-        port.dev = &d->dev;
+        uart.port.iotype = UPIO_MEM;
+        uart.port.flags = UPF_SKIP_TEST | UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF;
+        uart.port.irq = d->ipl;
+        uart.port.uartclk = HPDCA_BAUD_BASE * 16;
+        uart.port.mapbase = (d->resource.start + UART_OFFSET);
+        uart.port.membase = (char *)(uart.port.mapbase + DIO_VIRADDRBASE);
+        uart.port.regshift = 1;
+        uart.port.dev = &d->dev;
         line = serial8250_register_8250_port(&uart);
 
         if (line < 0) {
                 printk(KERN_NOTICE "8250_hp300: register_serial() DCA scode %d"
-                       " irq %d failed\n", d->scode, port.irq);
+                       " irq %d failed\n", d->scode, uart.port.irq);
                 return -ENOMEM;
         }
 
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 233fbaaf2559..2a53be5f010d 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -1150,7 +1150,7 @@ config SERIAL_SC26XX_CONSOLE
           Support for Console on SC2681/SC2692 serial ports.
 
 config SERIAL_SCCNXP
-        bool "SCCNXP serial port support"
+        tristate "SCCNXP serial port support"
         depends on !SERIAL_SC26XX
         select SERIAL_CORE
         default n
@@ -1162,7 +1162,7 @@ config SERIAL_SCCNXP
 
 config SERIAL_SCCNXP_CONSOLE
         bool "Console on SCCNXP serial port"
-        depends on SERIAL_SCCNXP
+        depends on SERIAL_SCCNXP=y
         select SERIAL_CORE_CONSOLE
         help
           Support for console on SCCNXP serial ports.
diff --git a/drivers/tty/serial/kgdboc.c b/drivers/tty/serial/kgdboc.c
index c0b334327d93..10020547c60b 100644
--- a/drivers/tty/serial/kgdboc.c
+++ b/drivers/tty/serial/kgdboc.c
@@ -97,7 +97,8 @@ static void kgdboc_restore_input(void)
 
 static int kgdboc_register_kbd(char **cptr)
 {
-        if (strncmp(*cptr, "kbd", 3) == 0) {
+        if (strncmp(*cptr, "kbd", 3) == 0 ||
+                strncmp(*cptr, "kdb", 3) == 0) {
                 if (kdb_poll_idx < KDB_POLL_FUNC_MAX) {
                         kdb_poll_funcs[kdb_poll_idx] = kdb_get_kbd_char;
                         kdb_poll_idx++;
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 6ede6fd92b4c..6d3d26a607b9 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -671,19 +671,19 @@ serial_omap_configure_xonxoff
 
         /*
          * IXON Flag:
-         * Flow control for OMAP.TX
-         * OMAP.RX should listen for XON/XOFF
+         * Enable XON/XOFF flow control on output.
+         * Transmit XON1, XOFF1
          */
         if (termios->c_iflag & IXON)
-                up->efr |= OMAP_UART_SW_RX;
+                up->efr |= OMAP_UART_SW_TX;
 
         /*
          * IXOFF Flag:
-         * Flow control for OMAP.RX
-         * OMAP.TX should send XON/XOFF
+         * Enable XON/XOFF flow control on input.
+         * Receiver compares XON1, XOFF1.
          */
         if (termios->c_iflag & IXOFF)
-                up->efr |= OMAP_UART_SW_TX;
+                up->efr |= OMAP_UART_SW_RX;
 
         serial_out(up, UART_EFR, up->efr | UART_EFR_ECB);
         serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index b7086d004f5f..e821068cd95b 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -971,6 +971,7 @@ static const struct platform_device_id sccnxp_id_table[] = {
         { "sc28202",    SCCNXP_TYPE_SC28202 },
         { "sc68681",    SCCNXP_TYPE_SC68681 },
         { "sc68692",    SCCNXP_TYPE_SC68692 },
+        { },
 };
 MODULE_DEVICE_TABLE(platform, sccnxp_id_table);
 
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 9be296cf7295..6ee59001d61d 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -530,7 +530,8 @@ static inline int sci_rxd_in(struct uart_port *port)
         if (s->cfg->port_reg <= 0)
                 return 1;
 
-        return !!__raw_readb(s->cfg->port_reg);
+        /* Cast for ARM damage */
+        return !!__raw_readb((void __iomem *)s->cfg->port_reg);
 }
 
 /* ********************************************************************** *
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index 05728894a88c..16ee6cee07da 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -452,6 +452,7 @@ static struct sysrq_key_op *sysrq_key_table[36] = {
         NULL,                           /* v */
         &sysrq_showstate_blocked_op,    /* w */
         /* x: May be registered on ppc/powerpc for xmon */
+        /* x: May be registered on sparc64 for global PMU dump */
         NULL,                           /* x */
         /* y: May be registered on sparc64 for global register dump */
         NULL,                           /* y */
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 999ca63afdef..f87d7e8964bf 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -3442,6 +3442,19 @@ int con_debug_enter(struct vc_data *vc)
                         kdb_set(2, setargs);
                 }
         }
+        if (vc->vc_cols < 999) {
+                int colcount;
+                char cols[4];
+                const char *setargs[3] = {
+                        "set",
+                        "COLUMNS",
+                        cols,
+                };
+                if (kdbgetintenv(setargs[0], &colcount)) {
+                        snprintf(cols, 4, "%i", vc->vc_cols);
+                        kdb_set(2, setargs);
+                }
+        }
 #endif /* CONFIG_KGDB_KDB */
         return ret;
 }
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 36f2be4def2f..6e49ec6f3adc 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -817,10 +817,6 @@ static const __u32 acm_tty_speed[] = {
         2500000, 3000000, 3500000, 4000000
 };
 
-static const __u8 acm_tty_size[] = {
-        5, 6, 7, 8
-};
-
 static void acm_tty_set_termios(struct tty_struct *tty,
                                                 struct ktermios *termios_old)
 {
@@ -834,7 +830,21 @@ static void acm_tty_set_termios(struct tty_struct *tty,
         newline.bParityType = termios->c_cflag & PARENB ?
                                 (termios->c_cflag & PARODD ? 1 : 2) +
                                 (termios->c_cflag & CMSPAR ? 2 : 0) : 0;
-        newline.bDataBits = acm_tty_size[(termios->c_cflag & CSIZE) >> 4];
+        switch (termios->c_cflag & CSIZE) {
+        case CS5:
+                newline.bDataBits = 5;
+                break;
+        case CS6:
+                newline.bDataBits = 6;
+                break;
+        case CS7:
+                newline.bDataBits = 7;
+                break;
+        case CS8:
+        default:
+                newline.bDataBits = 8;
+                break;
+        }
         /* FIXME: Needs to clear unsupported bits in the termios */
         acm->clocal = ((termios->c_cflag & CLOCAL) != 0);
 
@@ -1233,7 +1243,7 @@ made_compressed_probe:
 
                 if (usb_endpoint_xfer_int(epwrite))
                         usb_fill_int_urb(snd->urb, usb_dev,
-                                usb_sndbulkpipe(usb_dev, epwrite->bEndpointAddress),
+                                usb_sndintpipe(usb_dev, epwrite->bEndpointAddress),
                                 NULL, acm->writesize, acm_write_bulk, snd, epwrite->bInterval);
                 else
                         usb_fill_bulk_urb(snd->urb, usb_dev,
@@ -1551,6 +1561,9 @@ static const struct usb_device_id acm_ids[] = {
                                            Maybe we should define a new
                                            quirk for this. */
         },
+        { USB_DEVICE(0x0572, 0x1340), /* Conexant CX93010-2x UCMxx */
+        .driver_info = NO_UNION_NORMAL,
+        },
         { USB_DEVICE(0x1bbb, 0x0003), /* Alcatel OT-I650 */
         .driver_info = NO_UNION_NORMAL, /* reports zero length descriptor */
         },
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index e0356cb859b5..b78fbe222b72 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -1348,6 +1348,7 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
                                         ret = -EFAULT;
                                         goto error;
                                 }
+                                uurb->buffer += u;
                         }
                         totlen -= u;
                 }
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index ddd820d25288..6056db7af410 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -367,6 +367,10 @@ static int usb_probe_interface(struct device *dev)
         intf->condition = USB_INTERFACE_UNBOUND;
         usb_cancel_queued_reset(intf);
 
+        /* If the LPM disable succeeded, balance the ref counts. */
+        if (!lpm_disable_error)
+                usb_unlocked_enable_lpm(udev);
+
         /* Unbound interfaces are always runtime-PM-disabled and -suspended */
         if (driver->supports_autosuspend)
                 pm_runtime_disable(dev);
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 673ee4696262..1af04bdeaf0c 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -739,13 +739,16 @@ static void hub_tt_work(struct work_struct *work)
         int                     limit = 100;
 
         spin_lock_irqsave (&hub->tt.lock, flags);
-        while (--limit && !list_empty (&hub->tt.clear_list)) {
+        while (!list_empty(&hub->tt.clear_list)) {
                 struct list_head        *next;
                 struct usb_tt_clear     *clear;
                 struct usb_device       *hdev = hub->hdev;
                 const struct hc_driver  *drv;
                 int                     status;
 
+                if (!hub->quiescing && --limit < 0)
+                        break;
+
                 next = hub->tt.clear_list.next;
                 clear = list_entry (next, struct usb_tt_clear, clear_list);
                 list_del (&clear->clear_list);
@@ -1210,7 +1213,7 @@ static void hub_quiesce(struct usb_hub *hub, enum hub_quiescing_type type)
         if (hub->has_indicators)
                 cancel_delayed_work_sync(&hub->leds);
         if (hub->tt.hub)
-                cancel_work_sync(&hub->tt.clear_work);
+                flush_work(&hub->tt.clear_work);
 }
 
 /* caller has locked the hub device */
@@ -3241,8 +3244,7 @@ static int usb_req_set_sel(struct usb_device *udev, enum usb3_link_state state)
                         (state == USB3_LPM_U2 &&
                          (u2_sel > USB3_LPM_MAX_U2_SEL_PEL ||
                           u2_pel > USB3_LPM_MAX_U2_SEL_PEL))) {
-                dev_dbg(&udev->dev, "Device-initiated %s disabled due "
-                                "to long SEL %llu ms or PEL %llu ms\n",
+                dev_dbg(&udev->dev, "Device-initiated %s disabled due to long SEL %llu us or PEL %llu us\n",
                                 usb3_lpm_names[state], u1_sel, u1_pel);
                 return -EINVAL;
         }
@@ -3320,16 +3322,6 @@ static int usb_set_device_initiated_lpm(struct usb_device *udev,
 
         if (enable) {
                 /*
-                 * First, let the device know about the exit latencies
-                 * associated with the link state we're about to enable.
-                 */
-                ret = usb_req_set_sel(udev, state);
-                if (ret < 0) {
-                        dev_warn(&udev->dev, "Set SEL for device-initiated "
-                                        "%s failed.\n", usb3_lpm_names[state]);
-                        return -EBUSY;
-                }
-                /*
                  * Now send the control transfer to enable device-initiated LPM
                  * for either U1 or U2.
                  */
@@ -3414,7 +3406,28 @@ static int usb_set_lpm_timeout(struct usb_device *udev,
 static void usb_enable_link_state(struct usb_hcd *hcd, struct usb_device *udev,
                 enum usb3_link_state state)
 {
-        int timeout;
+        int timeout, ret;
+        __u8 u1_mel = udev->bos->ss_cap->bU1devExitLat;
+        __le16 u2_mel = udev->bos->ss_cap->bU2DevExitLat;
+
+        /* If the device says it doesn't have *any* exit latency to come out of
+         * U1 or U2, it's probably lying.  Assume it doesn't implement that link
+         * state.
+         */
+        if ((state == USB3_LPM_U1 && u1_mel == 0) ||
+                        (state == USB3_LPM_U2 && u2_mel == 0))
+                return;
+
+        /*
+         * First, let the device know about the exit latencies
+         * associated with the link state we're about to enable.
+         */
+        ret = usb_req_set_sel(udev, state);
+        if (ret < 0) {
+                dev_warn(&udev->dev, "Set SEL for device-initiated %s failed.\n",
+                                usb3_lpm_names[state]);
+                return;
+        }
 
         /* We allow the host controller to set the U1/U2 timeout internally
          * first, so that it can change its schedule to account for the
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index b415c0c859d3..c14ebc975ba4 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -409,6 +409,10 @@ static void dwc3_core_exit(struct dwc3 *dwc)
 {
         dwc3_event_buffers_cleanup(dwc);
         dwc3_free_event_buffers(dwc);
+
+        usb_phy_shutdown(dwc->usb2_phy);
+        usb_phy_shutdown(dwc->usb3_phy);
+
 }
 
 #define DWC3_ALIGN_MASK         (16 - 1)
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index c9e729a4bf65..7b7deddf6a52 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1904,7 +1904,7 @@ static void dwc3_stop_active_transfer(struct dwc3 *dwc, u32 epnum)
         ret = dwc3_send_gadget_ep_cmd(dwc, dep->number, cmd, &params);
         WARN_ON_ONCE(ret);
         dep->resource_index = 0;
-
+        dep->flags &= ~DWC3_EP_BUSY;
         udelay(100);
 }
 
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index dfb51a45496c..e0ff51b89529 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -952,6 +952,7 @@ endif
 config USB_G_WEBCAM
         tristate "USB Webcam Gadget"
         depends on VIDEO_DEV
+        select USB_LIBCOMPOSITE
         help
           The Webcam Gadget acts as a composite USB Audio and Video Class
           device. It provides a userspace API to process UVC control requests
diff --git a/drivers/usb/gadget/lpc32xx_udc.c b/drivers/usb/gadget/lpc32xx_udc.c
index f696fb9b136d..21a9861dabf0 100644
--- a/drivers/usb/gadget/lpc32xx_udc.c
+++ b/drivers/usb/gadget/lpc32xx_udc.c
@@ -2930,10 +2930,10 @@ static void vbus_work(struct work_struct *work)
 
                 /* Get the VBUS status from the transceiver */
                 value = i2c_smbus_read_byte_data(udc->isp1301_i2c_client,
-                                                 ISP1301_I2C_OTG_CONTROL_2);
+                                                 ISP1301_I2C_INTERRUPT_SOURCE);
 
                 /* VBUS on or off? */
-                if (value & OTG_B_SESS_VLD)
+                if (value & INT_SESS_VLD)
                         udc->vbus = 1;
                 else
                         udc->vbus = 0;
diff --git a/drivers/usb/gadget/net2272.c b/drivers/usb/gadget/net2272.c
index 43ac7482fa91..c009263a47e3 100644
--- a/drivers/usb/gadget/net2272.c
+++ b/drivers/usb/gadget/net2272.c
@@ -2069,8 +2069,10 @@ static irqreturn_t net2272_irq(int irq, void *_dev)
 #if defined(PLX_PCI_RDK2)
         /* see if PCI int for us by checking irqstat */
         intcsr = readl(dev->rdk2.fpga_base_addr + RDK2_IRQSTAT);
-        if (!intcsr & (1 << NET2272_PCI_IRQ))
+        if (!intcsr & (1 << NET2272_PCI_IRQ)) {
+                spin_unlock(&dev->lock);
                 return IRQ_NONE;
+        }
         /* check dma interrupts */
 #endif
         /* Platform/devcice interrupt handler */
diff --git a/drivers/usb/gadget/tcm_usb_gadget.c b/drivers/usb/gadget/tcm_usb_gadget.c
index eaa1005377fc..97e68b38cfdf 100644
--- a/drivers/usb/gadget/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/tcm_usb_gadget.c
@@ -1472,16 +1472,6 @@ static int usbg_queue_tm_rsp(struct se_cmd *se_cmd)
         return 0;
 }
 
-static u16 usbg_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-        return 0;
-}
-
-static u16 usbg_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
 static const char *usbg_check_wwn(const char *name)
 {
         const char *n;
@@ -1822,7 +1812,7 @@ static ssize_t tcm_usbg_tpg_store_nexus(
                 ret = tcm_usbg_drop_nexus(tpg);
                 return (!ret) ? count : ret;
         }
-        if (strlen(page) > USBG_NAMELEN) {
+        if (strlen(page) >= USBG_NAMELEN) {
                 pr_err("Emulated NAA Sas Address: %s, exceeds"
                                 " max: %d\n", page, USBG_NAMELEN);
                 return -EINVAL;
@@ -1907,8 +1897,6 @@ static struct target_core_fabric_ops usbg_ops = {
         .queue_data_in                  = usbg_send_read_response,
         .queue_status                   = usbg_send_status_response,
         .queue_tm_rsp                   = usbg_queue_tm_rsp,
-        .get_fabric_sense_len           = usbg_get_fabric_sense_len,
-        .set_fabric_sense_len           = usbg_set_fabric_sense_len,
         .check_stop_free                = usbg_check_stop_free,
 
         .fabric_make_wwn                = usbg_make_tport,
@@ -1968,7 +1956,6 @@ static void usbg_deregister_configfs(void)
 static struct usb_interface_descriptor bot_intf_desc = {
         .bLength =              sizeof(bot_intf_desc),
         .bDescriptorType =      USB_DT_INTERFACE,
-        .bAlternateSetting =    0,
         .bNumEndpoints =        2,
         .bAlternateSetting =    USB_G_ALT_INT_BBB,
         .bInterfaceClass =      USB_CLASS_MASS_STORAGE,
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 9bfde82078ec..0d2f35ca93f1 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -222,7 +222,7 @@ static int ehci_fsl_setup_phy(struct usb_hcd *hcd,
 
         if (pdata->controller_ver < 0) {
                 dev_warn(hcd->self.controller, "Could not get controller version\n");
-                return;
+                return -ENODEV;
         }
 
         portsc = ehci_readl(ehci, &ehci->regs->port_status[port_offset]);
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 8e7eca62f169..9c2717d66730 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -160,7 +160,7 @@ static const struct hc_driver ehci_orion_hc_driver = {
         .clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
 };
 
-static void __init
+static void __devinit
 ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
                              const struct mbus_dram_target_info *dram)
 {
diff --git a/drivers/usb/host/ehci-vt8500.c b/drivers/usb/host/ehci-vt8500.c
index 96722bfebc84..d3c9a3e397b9 100644
--- a/drivers/usb/host/ehci-vt8500.c
+++ b/drivers/usb/host/ehci-vt8500.c
@@ -85,6 +85,8 @@ static const struct hc_driver vt8500_ehci_hc_driver = {
         .clear_tt_buffer_complete       = ehci_clear_tt_buffer_complete,
 };
 
+static u64 vt8500_ehci_dma_mask = DMA_BIT_MASK(32);
+
 static int vt8500_ehci_drv_probe(struct platform_device *pdev)
 {
         struct usb_hcd *hcd;
@@ -95,6 +97,14 @@ static int vt8500_ehci_drv_probe(struct platform_device *pdev)
         if (usb_disabled())
                 return -ENODEV;
 
+        /*
+         * Right now device-tree probed devices don't get dma_mask set.
+         * Since shared usb code relies on it, set it here for now.
+         * Once we have dma capability bindings this can go away.
+         */
+        if (!pdev->dev.dma_mask)
+                pdev->dev.dma_mask = &vt8500_ehci_dma_mask;
+
         if (pdev->resource[1].flags != IORESOURCE_IRQ) {
                 pr_debug("resource[1] is not IORESOURCE_IRQ");
                 return -ENOMEM;
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 966d1484ee79..39f9e4a9a2d3 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -545,7 +545,14 @@ static const struct dmi_system_id __devinitconst ehci_dmi_nohandoff_table[] = {
                 /*  Pegatron Lucid (Ordissimo AIRIS) */
                 .matches = {
                         DMI_MATCH(DMI_BOARD_NAME, "M11JB"),
-                        DMI_MATCH(DMI_BIOS_VERSION, "Lucid-GE-133"),
+                        DMI_MATCH(DMI_BIOS_VERSION, "Lucid-"),
+                },
+        },
+        {
+                /*  Pegatron Lucid (Ordissimo) */
+                .matches = {
+                        DMI_MATCH(DMI_BOARD_NAME, "Ordissimo"),
+                        DMI_MATCH(DMI_BIOS_VERSION, "Lucid-"),
                 },
         },
         { }
diff --git a/drivers/usb/host/uhci-platform.c b/drivers/usb/host/uhci-platform.c
index e4780491df4a..68ebf20e1519 100644
--- a/drivers/usb/host/uhci-platform.c
+++ b/drivers/usb/host/uhci-platform.c
@@ -60,6 +60,7 @@ static const struct hc_driver uhci_platform_hc_driver = {
         .hub_control =          uhci_hub_control,
 };
 
+static u64 platform_uhci_dma_mask = DMA_BIT_MASK(32);
 
 static int __devinit uhci_hcd_platform_probe(struct platform_device *pdev)
 {
@@ -71,6 +72,14 @@ static int __devinit uhci_hcd_platform_probe(struct platform_device *pdev)
         if (usb_disabled())
                 return -ENODEV;
 
+        /*
+         * Right now device-tree probed devices don't get dma_mask set.
+         * Since shared usb code relies on it, set it here for now.
+         * Once we have dma capability bindings this can go away.
+         */
+        if (!pdev->dev.dma_mask)
+                pdev->dev.dma_mask = &platform_uhci_dma_mask;
+
         hcd = usb_create_hcd(&uhci_platform_hc_driver, &pdev->dev,
                         pdev->name);
         if (!hcd)
diff --git a/drivers/usb/host/xhci-dbg.c b/drivers/usb/host/xhci-dbg.c
index 4b436f5a4171..5f3a7c74aa8d 100644
--- a/drivers/usb/host/xhci-dbg.c
+++ b/drivers/usb/host/xhci-dbg.c
@@ -544,7 +544,6 @@ void xhci_dbg_ctx(struct xhci_hcd *xhci,
         int i;
         /* Fields are 32 bits wide, DMA addresses are in bytes */
         int field_size = 32 / 8;
-        struct xhci_slot_ctx *slot_ctx;
         dma_addr_t dma = ctx->dma;
         int csz = HCC_64BYTE_CONTEXT(xhci->hcc_params);
 
@@ -570,7 +569,6 @@ void xhci_dbg_ctx(struct xhci_hcd *xhci,
                         dbg_rsvd64(xhci, (u64 *)ctrl_ctx, dma);
         }
 
-        slot_ctx = xhci_get_slot_ctx(xhci, ctx);
         xhci_dbg_slot_ctx(xhci, ctx);
         xhci_dbg_ep_ctx(xhci, ctx, last_ep);
 }
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index aa90ad4d4fd5..a686cf4905bb 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -151,9 +151,8 @@ static void xhci_usb3_hub_descriptor(struct usb_hcd *hcd, struct xhci_hcd *xhci,
                 if (portsc & PORT_DEV_REMOVE)
                         port_removable |= 1 << (i + 1);
         }
-        memset(&desc->u.ss.DeviceRemovable,
-                        (__force __u16) cpu_to_le16(port_removable),
-                        sizeof(__u16));
+
+        desc->u.ss.DeviceRemovable = cpu_to_le16(port_removable);
 }
 
 static void xhci_hub_descriptor(struct usb_hcd *hcd, struct xhci_hcd *xhci,
@@ -809,11 +808,13 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                         temp = xhci_readl(xhci, port_array[wIndex]);
                         xhci_dbg(xhci, "set port power, actual port %d status  = 0x%x\n", wIndex, temp);
 
+                        spin_unlock_irqrestore(&xhci->lock, flags);
                         temp = usb_acpi_power_manageable(hcd->self.root_hub,
                                         wIndex);
                         if (temp)
                                 usb_acpi_set_power_state(hcd->self.root_hub,
                                                 wIndex, true);
+                        spin_lock_irqsave(&xhci->lock, flags);
                         break;
                 case USB_PORT_FEAT_RESET:
                         temp = (temp | PORT_RESET);
@@ -917,11 +918,13 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                         xhci_writel(xhci, temp & ~PORT_POWER,
                                 port_array[wIndex]);
 
+                        spin_unlock_irqrestore(&xhci->lock, flags);
                         temp = usb_acpi_power_manageable(hcd->self.root_hub,
                                         wIndex);
                         if (temp)
                                 usb_acpi_set_power_state(hcd->self.root_hub,
                                                 wIndex, false);
+                        spin_lock_irqsave(&xhci->lock, flags);
                         break;
                 default:
                         goto error;
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index c6ebb176dc4f..4e1a8946b8d1 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1228,6 +1228,17 @@ static void xhci_cmd_to_noop(struct xhci_hcd *xhci, struct xhci_cd *cur_cd)
         cur_seg = find_trb_seg(xhci->cmd_ring->first_seg,
                         xhci->cmd_ring->dequeue, &cycle_state);
 
+        if (!cur_seg) {
+                xhci_warn(xhci, "Command ring mismatch, dequeue = %p %llx (dma)\n",
+                                xhci->cmd_ring->dequeue,
+                                (unsigned long long)
+                                xhci_trb_virt_to_dma(xhci->cmd_ring->deq_seg,
+                                        xhci->cmd_ring->dequeue));
+                xhci_debug_ring(xhci, xhci->cmd_ring);
+                xhci_dbg_ring_ptrs(xhci, xhci->cmd_ring);
+                return;
+        }
+
         /* find the command trb matched by cd from command ring */
         for (cmd_trb = xhci->cmd_ring->dequeue;
                         cmd_trb != xhci->cmd_ring->enqueue;
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 8d7fcbbe6ade..c9e419f29b74 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -479,7 +479,8 @@ static bool compliance_mode_recovery_timer_quirk_check(void)
 
         if (strstr(dmi_product_name, "Z420") ||
                         strstr(dmi_product_name, "Z620") ||
-                        strstr(dmi_product_name, "Z820"))
+                        strstr(dmi_product_name, "Z820") ||
+                        strstr(dmi_product_name, "Z1"))
                 return true;
 
         return false;
@@ -1626,7 +1627,6 @@ int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
         struct xhci_hcd *xhci;
         struct xhci_container_ctx *in_ctx, *out_ctx;
         unsigned int ep_index;
-        struct xhci_ep_ctx *ep_ctx;
         struct xhci_slot_ctx *slot_ctx;
         struct xhci_input_control_ctx *ctrl_ctx;
         u32 added_ctxs;
@@ -1662,7 +1662,6 @@ int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
         out_ctx = virt_dev->out_ctx;
         ctrl_ctx = xhci_get_input_control_ctx(xhci, in_ctx);
         ep_index = xhci_get_endpoint_index(&ep->desc);
-        ep_ctx = xhci_get_ep_ctx(xhci, out_ctx, ep_index);
 
         /* If this endpoint is already in use, and the upper layers are trying
          * to add it again without dropping it, reject the addition.
@@ -1816,6 +1815,8 @@ static int xhci_evaluate_context_result(struct xhci_hcd *xhci,
         case COMP_EBADSLT:
                 dev_warn(&udev->dev, "WARN: slot not enabled for"
                                 "evaluate context command.\n");
+                ret = -EINVAL;
+                break;
         case COMP_CTX_STATE:
                 dev_warn(&udev->dev, "WARN: invalid context state for "
                                 "evaluate context command.\n");
@@ -4020,7 +4021,7 @@ int xhci_update_device(struct usb_hcd *hcd, struct usb_device *udev)
 static unsigned long long xhci_service_interval_to_ns(
                 struct usb_endpoint_descriptor *desc)
 {
-        return (1 << (desc->bInterval - 1)) * 125 * 1000;
+        return (1ULL << (desc->bInterval - 1)) * 125 * 1000;
 }
 
 static u16 xhci_get_timeout_no_hub_lpm(struct usb_device *udev,
@@ -4141,7 +4142,7 @@ static u16 xhci_calculate_intel_u2_timeout(struct usb_device *udev,
                         (xhci_service_interval_to_ns(desc) > timeout_ns))
                 timeout_ns = xhci_service_interval_to_ns(desc);
 
-        u2_del_ns = udev->bos->ss_cap->bU2DevExitLat * 1000;
+        u2_del_ns = le16_to_cpu(udev->bos->ss_cap->bU2DevExitLat) * 1000ULL;
         if (u2_del_ns > timeout_ns)
                 timeout_ns = u2_del_ns;
 
diff --git a/drivers/usb/misc/ezusb.c b/drivers/usb/misc/ezusb.c
index 4223d761223d..6589268a6515 100644
--- a/drivers/usb/misc/ezusb.c
+++ b/drivers/usb/misc/ezusb.c
@@ -158,3 +158,4 @@ int ezusb_fx2_ihex_firmware_download(struct usb_device *dev,
 }
 EXPORT_SYMBOL_GPL(ezusb_fx2_ihex_firmware_download);
 
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index 457f25e62c51..c964d6af178b 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -305,6 +305,12 @@ static irqreturn_t am35x_musb_interrupt(int irq, void *hci)
                 ret = IRQ_HANDLED;
         }
 
+        /* Drop spurious RX and TX if device is disconnected */
+        if (musb->int_usb & MUSB_INTR_DISCONNECT) {
+                musb->int_tx = 0;
+                musb->int_rx = 0;
+        }
+
         if (musb->int_tx || musb->int_rx || musb->int_usb)
                 ret |= musb_interrupt(musb);
 
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 444346e1e10d..ff5f112053d2 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -458,11 +458,11 @@ static int __devinit dsps_create_musb_pdev(struct dsps_glue *glue, u8 id)
         struct platform_device  *musb;
         struct resource *res;
         struct resource resources[2];
-        char res_name[10];
+        char res_name[11];
         int ret, musbid;
 
         /* get memory resource */
-        sprintf(res_name, "musb%d", id);
+        snprintf(res_name, sizeof(res_name), "musb%d", id);
         res = platform_get_resource_byname(pdev, IORESOURCE_MEM, res_name);
         if (!res) {
                 dev_err(dev, "%s get mem resource failed\n", res_name);
@@ -473,7 +473,7 @@ static int __devinit dsps_create_musb_pdev(struct dsps_glue *glue, u8 id)
         resources[0] = *res;
 
         /* get irq resource */
-        sprintf(res_name, "musb%d-irq", id);
+        snprintf(res_name, sizeof(res_name), "musb%d-irq", id);
         res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res_name);
         if (!res) {
                 dev_err(dev, "%s get irq resource failed\n", res_name);
@@ -530,7 +530,7 @@ static int __devinit dsps_create_musb_pdev(struct dsps_glue *glue, u8 id)
 
                 of_property_read_u32(np, "num-eps", (u32 *)&config->num_eps);
                 of_property_read_u32(np, "ram-bits", (u32 *)&config->ram_bits);
-                sprintf(res_name, "port%d-mode", id);
+                snprintf(res_name, sizeof(res_name), "port%d-mode", id);
                 of_property_read_u32(np, res_name, (u32 *)&pdata->mode);
                 of_property_read_u32(np, "power", (u32 *)&pdata->power);
                 config->multipoint = of_property_read_bool(np, "multipoint");
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index 143c4e9e1be4..c021b202c0f3 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -795,6 +795,7 @@ static void xfer_work(struct work_struct *work)
         dev_dbg(dev, "  %s %d (%d/ %d)\n",
                 fifo->name, usbhs_pipe_number(pipe), pkt->length, pkt->zero);
 
+        usbhs_pipe_enable(pipe);
         usbhsf_dma_start(pipe, fifo);
         dma_async_issue_pending(chan);
 }
diff --git a/drivers/usb/renesas_usbhs/mod.c b/drivers/usb/renesas_usbhs/mod.c
index 35c5208f3249..61933a90e5bf 100644
--- a/drivers/usb/renesas_usbhs/mod.c
+++ b/drivers/usb/renesas_usbhs/mod.c
@@ -273,9 +273,9 @@ static irqreturn_t usbhs_interrupt(int irq, void *data)
         usbhs_write(priv, INTSTS0, ~irq_state.intsts0 & INTSTS0_MAGIC);
         usbhs_write(priv, INTSTS1, ~irq_state.intsts1 & INTSTS1_MAGIC);
 
-        usbhs_write(priv, BRDYSTS, 0);
-        usbhs_write(priv, NRDYSTS, 0);
-        usbhs_write(priv, BEMPSTS, 0);
+        usbhs_write(priv, BRDYSTS, ~irq_state.brdysts);
+        usbhs_write(priv, NRDYSTS, ~irq_state.nrdysts);
+        usbhs_write(priv, BEMPSTS, ~irq_state.bempsts);
 
         /*
          * call irq callback functions
diff --git a/drivers/usb/renesas_usbhs/mod_host.c b/drivers/usb/renesas_usbhs/mod_host.c
index 9b69a1323294..069cd765400c 100644
--- a/drivers/usb/renesas_usbhs/mod_host.c
+++ b/drivers/usb/renesas_usbhs/mod_host.c
@@ -334,6 +334,11 @@ static void usbhsh_pipe_detach(struct usbhsh_hpriv *hpriv,
         struct device *dev = usbhs_priv_to_dev(priv);
         unsigned long flags;
 
+        if (unlikely(!uep)) {
+                dev_err(dev, "no uep\n");
+                return;
+        }
+
         /********************  spin lock ********************/
         usbhs_lock(priv, flags);
 
diff --git a/drivers/usb/renesas_usbhs/pipe.h b/drivers/usb/renesas_usbhs/pipe.h
index 08786c06dcf1..3d80c7b1fd1b 100644
--- a/drivers/usb/renesas_usbhs/pipe.h
+++ b/drivers/usb/renesas_usbhs/pipe.h
@@ -54,7 +54,7 @@ struct usbhs_pipe_info {
  * pipe list
  */
 #define __usbhs_for_each_pipe(start, pos, info, i)      \
-        for (i = start, pos = (info)->pipe;             \
+        for (i = start, pos = (info)->pipe + i;         \
              i < (info)->size;                          \
              i++, pos = (info)->pipe + i)
 
diff --git a/drivers/usb/serial/ark3116.c b/drivers/usb/serial/ark3116.c
index cf2522c397d3..bd50a8a41a0f 100644
--- a/drivers/usb/serial/ark3116.c
+++ b/drivers/usb/serial/ark3116.c
@@ -125,9 +125,6 @@ static inline int calc_divisor(int bps)
 
 static int ark3116_attach(struct usb_serial *serial)
 {
-        struct usb_serial_port *port = serial->port[0];
-        struct ark3116_private *priv;
-
         /* make sure we have our end-points */
         if ((serial->num_bulk_in == 0) ||
             (serial->num_bulk_out == 0) ||
@@ -142,8 +139,15 @@ static int ark3116_attach(struct usb_serial *serial)
                 return -EINVAL;
         }
 
-        priv = kzalloc(sizeof(struct ark3116_private),
-                       GFP_KERNEL);
+        return 0;
+}
+
+static int ark3116_port_probe(struct usb_serial_port *port)
+{
+        struct usb_serial *serial = port->serial;
+        struct ark3116_private *priv;
+
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
         if (!priv)
                 return -ENOMEM;
 
@@ -198,18 +202,15 @@ static int ark3116_attach(struct usb_serial *serial)
         return 0;
 }
 
-static void ark3116_release(struct usb_serial *serial)
+static int ark3116_port_remove(struct usb_serial_port *port)
 {
-        struct usb_serial_port *port = serial->port[0];
         struct ark3116_private *priv = usb_get_serial_port_data(port);
 
         /* device is closed, so URBs and DMA should be down */
-
-        usb_set_serial_port_data(port, NULL);
-
         mutex_destroy(&priv->hw_lock);
-
         kfree(priv);
+
+        return 0;
 }
 
 static void ark3116_init_termios(struct tty_struct *tty)
@@ -723,7 +724,8 @@ static struct usb_serial_driver ark3116_device = {
         .id_table =             id_table,
         .num_ports =            1,
         .attach =               ark3116_attach,
-        .release =              ark3116_release,
+        .port_probe =           ark3116_port_probe,
+        .port_remove =          ark3116_port_remove,
         .set_termios =          ark3116_set_termios,
         .init_termios =         ark3116_init_termios,
         .ioctl =                ark3116_ioctl,
diff --git a/drivers/usb/serial/belkin_sa.c b/drivers/usb/serial/belkin_sa.c
index 99449424193f..ea29556f0d72 100644
--- a/drivers/usb/serial/belkin_sa.c
+++ b/drivers/usb/serial/belkin_sa.c
@@ -45,8 +45,8 @@
 #define DRIVER_DESC "USB Belkin Serial converter driver"
 
 /* function prototypes for a Belkin USB Serial Adapter F5U103 */
-static int  belkin_sa_startup(struct usb_serial *serial);
-static void belkin_sa_release(struct usb_serial *serial);
+static int belkin_sa_port_probe(struct usb_serial_port *port);
+static int belkin_sa_port_remove(struct usb_serial_port *port);
 static int  belkin_sa_open(struct tty_struct *tty,
                         struct usb_serial_port *port);
 static void belkin_sa_close(struct usb_serial_port *port);
@@ -88,8 +88,8 @@ static struct usb_serial_driver belkin_device = {
         .break_ctl =            belkin_sa_break_ctl,
         .tiocmget =             belkin_sa_tiocmget,
         .tiocmset =             belkin_sa_tiocmset,
-        .attach =               belkin_sa_startup,
-        .release =              belkin_sa_release,
+        .port_probe =           belkin_sa_port_probe,
+        .port_remove =          belkin_sa_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -118,17 +118,15 @@ struct belkin_sa_private {
                                             (c), BELKIN_SA_SET_REQUEST_TYPE, \
                                             (v), 0, NULL, 0, WDR_TIMEOUT)
 
-/* do some startup allocations not currently performed by usb_serial_probe() */
-static int belkin_sa_startup(struct usb_serial *serial)
+static int belkin_sa_port_probe(struct usb_serial_port *port)
 {
-        struct usb_device *dev = serial->dev;
+        struct usb_device *dev = port->serial->dev;
         struct belkin_sa_private *priv;
 
-        /* allocate the private data structure */
         priv = kmalloc(sizeof(struct belkin_sa_private), GFP_KERNEL);
         if (!priv)
-                return -1; /* error */
-        /* set initial values for control structures */
+                return -ENOMEM;
+
         spin_lock_init(&priv->lock);
         priv->control_state = 0;
         priv->last_lsr = 0;
@@ -140,18 +138,19 @@ static int belkin_sa_startup(struct usb_serial *serial)
                                         le16_to_cpu(dev->descriptor.bcdDevice),
                                         priv->bad_flow_control);
 
-        init_waitqueue_head(&serial->port[0]->write_wait);
-        usb_set_serial_port_data(serial->port[0], priv);
+        usb_set_serial_port_data(port, priv);
 
         return 0;
 }
 
-static void belkin_sa_release(struct usb_serial *serial)
+static int belkin_sa_port_remove(struct usb_serial_port *port)
 {
-        int i;
+        struct belkin_sa_private *priv;
 
-        for (i = 0; i < serial->num_ports; ++i)
-                kfree(usb_get_serial_port_data(serial->port[i]));
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 static int belkin_sa_open(struct tty_struct *tty,
diff --git a/drivers/usb/serial/ch341.c b/drivers/usb/serial/ch341.c
index e9c7046ae355..d255f66e708e 100644
--- a/drivers/usb/serial/ch341.c
+++ b/drivers/usb/serial/ch341.c
@@ -242,13 +242,11 @@ out:	kfree(buffer);
         return r;
 }
 
-/* allocate private data */
-static int ch341_attach(struct usb_serial *serial)
+static int ch341_port_probe(struct usb_serial_port *port)
 {
         struct ch341_private *priv;
         int r;
 
-        /* private data */
         priv = kzalloc(sizeof(struct ch341_private), GFP_KERNEL);
         if (!priv)
                 return -ENOMEM;
@@ -258,17 +256,27 @@ static int ch341_attach(struct usb_serial *serial)
         priv->baud_rate = DEFAULT_BAUD_RATE;
         priv->line_control = CH341_BIT_RTS | CH341_BIT_DTR;
 
-        r = ch341_configure(serial->dev, priv);
+        r = ch341_configure(port->serial->dev, priv);
         if (r < 0)
                 goto error;
 
-        usb_set_serial_port_data(serial->port[0], priv);
+        usb_set_serial_port_data(port, priv);
         return 0;
 
 error:  kfree(priv);
         return r;
 }
 
+static int ch341_port_remove(struct usb_serial_port *port)
+{
+        struct ch341_private *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
+}
+
 static int ch341_carrier_raised(struct usb_serial_port *port)
 {
         struct ch341_private *priv = usb_get_serial_port_data(port);
@@ -304,7 +312,7 @@ static void ch341_close(struct usb_serial_port *port)
 static int ch341_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
         struct usb_serial *serial = port->serial;
-        struct ch341_private *priv = usb_get_serial_port_data(serial->port[0]);
+        struct ch341_private *priv = usb_get_serial_port_data(port);
         int r;
 
         priv->baud_rate = DEFAULT_BAUD_RATE;
@@ -608,7 +616,8 @@ static struct usb_serial_driver ch341_device = {
         .tiocmget          = ch341_tiocmget,
         .tiocmset          = ch341_tiocmset,
         .read_int_callback = ch341_read_int_callback,
-        .attach            = ch341_attach,
+        .port_probe        = ch341_port_probe,
+        .port_remove       = ch341_port_remove,
         .reset_resume      = ch341_reset_resume,
 };
 
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 28af5acc3360..eb033fc92a15 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -162,7 +162,7 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-struct cp210x_port_private {
+struct cp210x_serial_private {
         __u8                    bInterfaceNumber;
 };
 
@@ -276,7 +276,7 @@ static int cp210x_get_config(struct usb_serial_port *port, u8 request,
                 unsigned int *data, int size)
 {
         struct usb_serial *serial = port->serial;
-        struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
+        struct cp210x_serial_private *spriv = usb_get_serial_data(serial);
         __le32 *buf;
         int result, i, length;
 
@@ -292,7 +292,7 @@ static int cp210x_get_config(struct usb_serial_port *port, u8 request,
         /* Issue the request, attempting to read 'size' bytes */
         result = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
                                 request, REQTYPE_INTERFACE_TO_HOST, 0x0000,
-                                port_priv->bInterfaceNumber, buf, size,
+                                spriv->bInterfaceNumber, buf, size,
                                 USB_CTRL_GET_TIMEOUT);
 
         /* Convert data into an array of integers */
@@ -323,7 +323,7 @@ static int cp210x_set_config(struct usb_serial_port *port, u8 request,
                 unsigned int *data, int size)
 {
         struct usb_serial *serial = port->serial;
-        struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
+        struct cp210x_serial_private *spriv = usb_get_serial_data(serial);
         __le32 *buf;
         int result, i, length;
 
@@ -345,13 +345,13 @@ static int cp210x_set_config(struct usb_serial_port *port, u8 request,
                 result = usb_control_msg(serial->dev,
                                 usb_sndctrlpipe(serial->dev, 0),
                                 request, REQTYPE_HOST_TO_INTERFACE, 0x0000,
-                                port_priv->bInterfaceNumber, buf, size,
+                                spriv->bInterfaceNumber, buf, size,
                                 USB_CTRL_SET_TIMEOUT);
         } else {
                 result = usb_control_msg(serial->dev,
                                 usb_sndctrlpipe(serial->dev, 0),
                                 request, REQTYPE_HOST_TO_INTERFACE, data[0],
-                                port_priv->bInterfaceNumber, NULL, 0,
+                                spriv->bInterfaceNumber, NULL, 0,
                                 USB_CTRL_SET_TIMEOUT);
         }
 
@@ -845,36 +845,30 @@ static void cp210x_break_ctl (struct tty_struct *tty, int break_state)
 
 static int cp210x_startup(struct usb_serial *serial)
 {
-        struct cp210x_port_private *port_priv;
-        int i;
+        struct usb_host_interface *cur_altsetting;
+        struct cp210x_serial_private *spriv;
 
         /* cp210x buffers behave strangely unless device is reset */
         usb_reset_device(serial->dev);
 
-        for (i = 0; i < serial->num_ports; i++) {
-                port_priv = kzalloc(sizeof(*port_priv), GFP_KERNEL);
-                if (!port_priv)
-                        return -ENOMEM;
+        spriv = kzalloc(sizeof(*spriv), GFP_KERNEL);
+        if (!spriv)
+                return -ENOMEM;
 
-                port_priv->bInterfaceNumber =
-                    serial->interface->cur_altsetting->desc.bInterfaceNumber;
+        cur_altsetting = serial->interface->cur_altsetting;
+        spriv->bInterfaceNumber = cur_altsetting->desc.bInterfaceNumber;
 
-                usb_set_serial_port_data(serial->port[i], port_priv);
-        }
+        usb_set_serial_data(serial, spriv);
 
         return 0;
 }
 
 static void cp210x_release(struct usb_serial *serial)
 {
-        struct cp210x_port_private *port_priv;
-        int i;
+        struct cp210x_serial_private *spriv;
 
-        for (i = 0; i < serial->num_ports; i++) {
-                port_priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(port_priv);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
+        spriv = usb_get_serial_data(serial);
+        kfree(spriv);
 }
 
 module_usb_serial_driver(serial_drivers, id_table);
diff --git a/drivers/usb/serial/cyberjack.c b/drivers/usb/serial/cyberjack.c
index 2a7aecc72237..4ee77dcbe690 100644
--- a/drivers/usb/serial/cyberjack.c
+++ b/drivers/usb/serial/cyberjack.c
@@ -55,9 +55,9 @@
 #define CYBERJACK_PRODUCT_ID    0x0100
 
 /* Function prototypes */
-static int cyberjack_startup(struct usb_serial *serial);
 static void cyberjack_disconnect(struct usb_serial *serial);
-static void cyberjack_release(struct usb_serial *serial);
+static int cyberjack_port_probe(struct usb_serial_port *port);
+static int cyberjack_port_remove(struct usb_serial_port *port);
 static int  cyberjack_open(struct tty_struct *tty,
         struct usb_serial_port *port);
 static void cyberjack_close(struct usb_serial_port *port);
@@ -83,9 +83,9 @@ static struct usb_serial_driver cyberjack_device = {
         .description =          "Reiner SCT Cyberjack USB card reader",
         .id_table =             id_table,
         .num_ports =            1,
-        .attach =               cyberjack_startup,
         .disconnect =           cyberjack_disconnect,
-        .release =              cyberjack_release,
+        .port_probe =           cyberjack_port_probe,
+        .port_remove =          cyberjack_port_remove,
         .open =                 cyberjack_open,
         .close =                cyberjack_close,
         .write =                cyberjack_write,
@@ -107,56 +107,45 @@ struct cyberjack_private {
         short           wrsent;         /* Data already sent */
 };
 
-/* do some startup allocations not currently performed by usb_serial_probe() */
-static int cyberjack_startup(struct usb_serial *serial)
+static int cyberjack_port_probe(struct usb_serial_port *port)
 {
         struct cyberjack_private *priv;
-        int i;
+        int result;
 
-        /* allocate the private data structure */
         priv = kmalloc(sizeof(struct cyberjack_private), GFP_KERNEL);
         if (!priv)
                 return -ENOMEM;
 
-        /* set initial values */
         spin_lock_init(&priv->lock);
         priv->rdtodo = 0;
         priv->wrfilled = 0;
         priv->wrsent = 0;
-        usb_set_serial_port_data(serial->port[0], priv);
 
-        init_waitqueue_head(&serial->port[0]->write_wait);
+        usb_set_serial_port_data(port, priv);
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                int result;
-                result = usb_submit_urb(serial->port[i]->interrupt_in_urb,
-                                        GFP_KERNEL);
-                if (result)
-                        dev_err(&serial->dev->dev,
-                                "usb_submit_urb(read int) failed\n");
-                dev_dbg(&serial->dev->dev, "%s - usb_submit_urb(int urb)\n",
-                        __func__);
-        }
+        result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
+        if (result)
+                dev_err(&port->dev, "usb_submit_urb(read int) failed\n");
 
         return 0;
 }
 
-static void cyberjack_disconnect(struct usb_serial *serial)
+static int cyberjack_port_remove(struct usb_serial_port *port)
 {
-        int i;
+        struct cyberjack_private *priv;
 
-        for (i = 0; i < serial->num_ports; ++i)
-                usb_kill_urb(serial->port[i]->interrupt_in_urb);
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
-static void cyberjack_release(struct usb_serial *serial)
+static void cyberjack_disconnect(struct usb_serial *serial)
 {
         int i;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                /* My special items, the standard routines free my urbs */
-                kfree(usb_get_serial_port_data(serial->port[i]));
-        }
+        for (i = 0; i < serial->num_ports; ++i)
+                usb_kill_urb(serial->port[i]->interrupt_in_urb);
 }
 
 static int  cyberjack_open(struct tty_struct *tty,
diff --git a/drivers/usb/serial/cypress_m8.c b/drivers/usb/serial/cypress_m8.c
index 1befce21e173..f0da1279c114 100644
--- a/drivers/usb/serial/cypress_m8.c
+++ b/drivers/usb/serial/cypress_m8.c
@@ -123,10 +123,10 @@ struct cypress_private {
 };
 
 /* function prototypes for the Cypress USB to serial device */
-static int  cypress_earthmate_startup(struct usb_serial *serial);
-static int  cypress_hidcom_startup(struct usb_serial *serial);
-static int  cypress_ca42v2_startup(struct usb_serial *serial);
-static void cypress_release(struct usb_serial *serial);
+static int  cypress_earthmate_port_probe(struct usb_serial_port *port);
+static int  cypress_hidcom_port_probe(struct usb_serial_port *port);
+static int  cypress_ca42v2_port_probe(struct usb_serial_port *port);
+static int  cypress_port_remove(struct usb_serial_port *port);
 static int  cypress_open(struct tty_struct *tty, struct usb_serial_port *port);
 static void cypress_close(struct usb_serial_port *port);
 static void cypress_dtr_rts(struct usb_serial_port *port, int on);
@@ -156,8 +156,8 @@ static struct usb_serial_driver cypress_earthmate_device = {
         .description =                  "DeLorme Earthmate USB",
         .id_table =                     id_table_earthmate,
         .num_ports =                    1,
-        .attach =                       cypress_earthmate_startup,
-        .release =                      cypress_release,
+        .port_probe =                   cypress_earthmate_port_probe,
+        .port_remove =                  cypress_port_remove,
         .open =                         cypress_open,
         .close =                        cypress_close,
         .dtr_rts =                      cypress_dtr_rts,
@@ -182,8 +182,8 @@ static struct usb_serial_driver cypress_hidcom_device = {
         .description =                  "HID->COM RS232 Adapter",
         .id_table =                     id_table_cyphidcomrs232,
         .num_ports =                    1,
-        .attach =                       cypress_hidcom_startup,
-        .release =                      cypress_release,
+        .port_probe =                   cypress_hidcom_port_probe,
+        .port_remove =                  cypress_port_remove,
         .open =                         cypress_open,
         .close =                        cypress_close,
         .dtr_rts =                      cypress_dtr_rts,
@@ -208,8 +208,8 @@ static struct usb_serial_driver cypress_ca42v2_device = {
         .description =                  "Nokia CA-42 V2 Adapter",
         .id_table =                     id_table_nokiaca42v2,
         .num_ports =                    1,
-        .attach =                       cypress_ca42v2_startup,
-        .release =                      cypress_release,
+        .port_probe =                   cypress_ca42v2_port_probe,
+        .port_remove =                  cypress_port_remove,
         .open =                         cypress_open,
         .close =                        cypress_close,
         .dtr_rts =                      cypress_dtr_rts,
@@ -438,10 +438,10 @@ static void cypress_set_dead(struct usb_serial_port *port)
  *****************************************************************************/
 
 
-static int generic_startup(struct usb_serial *serial)
+static int cypress_generic_port_probe(struct usb_serial_port *port)
 {
+        struct usb_serial *serial = port->serial;
         struct cypress_private *priv;
-        struct usb_serial_port *port = serial->port[0];
 
         priv = kzalloc(sizeof(struct cypress_private), GFP_KERNEL);
         if (!priv)
@@ -490,14 +490,16 @@ static int generic_startup(struct usb_serial *serial)
 }
 
 
-static int cypress_earthmate_startup(struct usb_serial *serial)
+static int cypress_earthmate_port_probe(struct usb_serial_port *port)
 {
+        struct usb_serial *serial = port->serial;
         struct cypress_private *priv;
-        struct usb_serial_port *port = serial->port[0];
+        int ret;
 
-        if (generic_startup(serial)) {
+        ret = cypress_generic_port_probe(port);
+        if (ret) {
                 dev_dbg(&port->dev, "%s - Failed setting up port\n", __func__);
-                return 1;
+                return ret;
         }
 
         priv = usb_get_serial_port_data(port);
@@ -518,56 +520,53 @@ static int cypress_earthmate_startup(struct usb_serial *serial)
         }
 
         return 0;
-} /* cypress_earthmate_startup */
-
+}
 
-static int cypress_hidcom_startup(struct usb_serial *serial)
+static int cypress_hidcom_port_probe(struct usb_serial_port *port)
 {
         struct cypress_private *priv;
-        struct usb_serial_port *port = serial->port[0];
+        int ret;
 
-        if (generic_startup(serial)) {
+        ret = cypress_generic_port_probe(port);
+        if (ret) {
                 dev_dbg(&port->dev, "%s - Failed setting up port\n", __func__);
-                return 1;
+                return ret;
         }
 
         priv = usb_get_serial_port_data(port);
         priv->chiptype = CT_CYPHIDCOM;
 
         return 0;
-} /* cypress_hidcom_startup */
-
+}
 
-static int cypress_ca42v2_startup(struct usb_serial *serial)
+static int cypress_ca42v2_port_probe(struct usb_serial_port *port)
 {
         struct cypress_private *priv;
-        struct usb_serial_port *port = serial->port[0];
+        int ret;
 
-        if (generic_startup(serial)) {
+        ret = cypress_generic_port_probe(port);
+        if (ret) {
                 dev_dbg(&port->dev, "%s - Failed setting up port\n", __func__);
-                return 1;
+                return ret;
         }
 
         priv = usb_get_serial_port_data(port);
         priv->chiptype = CT_CA42V2;
 
         return 0;
-} /* cypress_ca42v2_startup */
-
+}
 
-static void cypress_release(struct usb_serial *serial)
+static int cypress_port_remove(struct usb_serial_port *port)
 {
         struct cypress_private *priv;
 
-        /* all open ports are closed at this point */
-        priv = usb_get_serial_port_data(serial->port[0]);
+        priv = usb_get_serial_port_data(port);
 
-        if (priv) {
-                kfifo_free(&priv->write_fifo);
-                kfree(priv);
-        }
-}
+        kfifo_free(&priv->write_fifo);
+        kfree(priv);
 
+        return 0;
+}
 
 static int cypress_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index c86f68c6b078..b50fa1c6d885 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -244,6 +244,8 @@ static int digi_startup_device(struct usb_serial *serial);
 static int digi_startup(struct usb_serial *serial);
 static void digi_disconnect(struct usb_serial *serial);
 static void digi_release(struct usb_serial *serial);
+static int digi_port_probe(struct usb_serial_port *port);
+static int digi_port_remove(struct usb_serial_port *port);
 static void digi_read_bulk_callback(struct urb *urb);
 static int digi_read_inb_callback(struct urb *urb);
 static int digi_read_oob_callback(struct urb *urb);
@@ -294,6 +296,8 @@ static struct usb_serial_driver digi_acceleport_2_device = {
         .attach =                       digi_startup,
         .disconnect =                   digi_disconnect,
         .release =                      digi_release,
+        .port_probe =                   digi_port_probe,
+        .port_remove =                  digi_port_remove,
 };
 
 static struct usb_serial_driver digi_acceleport_4_device = {
@@ -320,6 +324,8 @@ static struct usb_serial_driver digi_acceleport_4_device = {
         .attach =                       digi_startup,
         .disconnect =                   digi_disconnect,
         .release =                      digi_release,
+        .port_probe =                   digi_port_probe,
+        .port_remove =                  digi_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -1240,59 +1246,50 @@ static int digi_startup_device(struct usb_serial *serial)
         return ret;
 }
 
-
-static int digi_startup(struct usb_serial *serial)
+static int digi_port_init(struct usb_serial_port *port, unsigned port_num)
 {
-
-        int i;
         struct digi_port *priv;
-        struct digi_serial *serial_priv;
 
-        /* allocate the private data structures for all ports */
-        /* number of regular ports + 1 for the out-of-band port */
-        for (i = 0; i < serial->type->num_ports + 1; i++) {
-                /* allocate port private structure */
-                priv = kmalloc(sizeof(struct digi_port), GFP_KERNEL);
-                if (priv == NULL) {
-                        while (--i >= 0)
-                                kfree(usb_get_serial_port_data(serial->port[i]));
-                        return 1;                       /* error */
-                }
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
 
-                /* initialize port private structure */
-                spin_lock_init(&priv->dp_port_lock);
-                priv->dp_port_num = i;
-                priv->dp_out_buf_len = 0;
-                priv->dp_write_urb_in_use = 0;
-                priv->dp_modem_signals = 0;
-                init_waitqueue_head(&priv->dp_modem_change_wait);
-                priv->dp_transmit_idle = 0;
-                init_waitqueue_head(&priv->dp_transmit_idle_wait);
-                priv->dp_throttled = 0;
-                priv->dp_throttle_restart = 0;
-                init_waitqueue_head(&priv->dp_flush_wait);
-                init_waitqueue_head(&priv->dp_close_wait);
-                INIT_WORK(&priv->dp_wakeup_work, digi_wakeup_write_lock);
-                priv->dp_port = serial->port[i];
-                /* initialize write wait queue for this port */
-                init_waitqueue_head(&serial->port[i]->write_wait);
-
-                usb_set_serial_port_data(serial->port[i], priv);
-        }
+        spin_lock_init(&priv->dp_port_lock);
+        priv->dp_port_num = port_num;
+        init_waitqueue_head(&priv->dp_modem_change_wait);
+        init_waitqueue_head(&priv->dp_transmit_idle_wait);
+        init_waitqueue_head(&priv->dp_flush_wait);
+        init_waitqueue_head(&priv->dp_close_wait);
+        INIT_WORK(&priv->dp_wakeup_work, digi_wakeup_write_lock);
+        priv->dp_port = port;
 
-        /* allocate serial private structure */
-        serial_priv = kmalloc(sizeof(struct digi_serial), GFP_KERNEL);
-        if (serial_priv == NULL) {
-                for (i = 0; i < serial->type->num_ports + 1; i++)
-                        kfree(usb_get_serial_port_data(serial->port[i]));
-                return 1;                       /* error */
-        }
+        init_waitqueue_head(&port->write_wait);
+
+        usb_set_serial_port_data(port, priv);
+
+        return 0;
+}
+
+static int digi_startup(struct usb_serial *serial)
+{
+        struct digi_serial *serial_priv;
+        int ret;
+
+        serial_priv = kzalloc(sizeof(*serial_priv), GFP_KERNEL);
+        if (!serial_priv)
+                return -ENOMEM;
 
-        /* initialize serial private structure */
         spin_lock_init(&serial_priv->ds_serial_lock);
         serial_priv->ds_oob_port_num = serial->type->num_ports;
         serial_priv->ds_oob_port = serial->port[serial_priv->ds_oob_port_num];
-        serial_priv->ds_device_started = 0;
+
+        ret = digi_port_init(serial_priv->ds_oob_port,
+                                                serial_priv->ds_oob_port_num);
+        if (ret) {
+                kfree(serial_priv);
+                return ret;
+        }
+
         usb_set_serial_data(serial, serial_priv);
 
         return 0;
@@ -1313,15 +1310,35 @@ static void digi_disconnect(struct usb_serial *serial)
 
 static void digi_release(struct usb_serial *serial)
 {
-        int i;
+        struct digi_serial *serial_priv;
+        struct digi_port *priv;
+
+        serial_priv = usb_get_serial_data(serial);
+
+        priv = usb_get_serial_port_data(serial_priv->ds_oob_port);
+        kfree(priv);
 
-        /* free the private data structures for all ports */
-        /* number of regular ports + 1 for the out-of-band port */
-        for (i = 0; i < serial->type->num_ports + 1; i++)
-                kfree(usb_get_serial_port_data(serial->port[i]));
-        kfree(usb_get_serial_data(serial));
+        kfree(serial_priv);
 }
 
+static int digi_port_probe(struct usb_serial_port *port)
+{
+        unsigned port_num;
+
+        port_num = port->number - port->serial->minor;
+
+        return digi_port_init(port, port_num);
+}
+
+static int digi_port_remove(struct usb_serial_port *port)
+{
+        struct digi_port *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
+}
 
 static void digi_read_bulk_callback(struct urb *urb)
 {
diff --git a/drivers/usb/serial/f81232.c b/drivers/usb/serial/f81232.c
index 244477107e2f..6e4eb57d0177 100644
--- a/drivers/usb/serial/f81232.c
+++ b/drivers/usb/serial/f81232.c
@@ -318,39 +318,30 @@ static int f81232_ioctl(struct tty_struct *tty,
         return -ENOIOCTLCMD;
 }
 
-static int f81232_startup(struct usb_serial *serial)
+static int f81232_port_probe(struct usb_serial_port *port)
 {
         struct f81232_private *priv;
-        int i;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                priv = kzalloc(sizeof(struct f81232_private), GFP_KERNEL);
-                if (!priv)
-                        goto cleanup;
-                spin_lock_init(&priv->lock);
-                init_waitqueue_head(&priv->delta_msr_wait);
-                usb_set_serial_port_data(serial->port[i], priv);
-        }
-        return 0;
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
 
-cleanup:
-        for (--i; i >= 0; --i) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
-        return -ENOMEM;
+        spin_lock_init(&priv->lock);
+        init_waitqueue_head(&priv->delta_msr_wait);
+
+        usb_set_serial_port_data(port, priv);
+
+        return 0;
 }
 
-static void f81232_release(struct usb_serial *serial)
+static int f81232_port_remove(struct usb_serial_port *port)
 {
-        int i;
         struct f81232_private *priv;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-        }
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 static struct usb_serial_driver f81232_device = {
@@ -373,8 +364,8 @@ static struct usb_serial_driver f81232_device = {
         .tiocmset =             f81232_tiocmset,
         .process_read_urb =     f81232_process_read_urb,
         .read_int_callback =    f81232_read_int_callback,
-        .attach =               f81232_startup,
-        .release =              f81232_release,
+        .port_probe =           f81232_port_probe,
+        .port_remove =          f81232_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/garmin_gps.c b/drivers/usb/serial/garmin_gps.c
index 3ee92648c02d..203358d7e7bc 100644
--- a/drivers/usb/serial/garmin_gps.c
+++ b/drivers/usb/serial/garmin_gps.c
@@ -1405,11 +1405,10 @@ static void timeout_handler(unsigned long data)
 
 
 
-static int garmin_attach(struct usb_serial *serial)
+static int garmin_port_probe(struct usb_serial_port *port)
 {
-        int status = 0;
-        struct usb_serial_port *port = serial->port[0];
-        struct garmin_data *garmin_data_p = NULL;
+        int status;
+        struct garmin_data *garmin_data_p;
 
         garmin_data_p = kzalloc(sizeof(struct garmin_data), GFP_KERNEL);
         if (garmin_data_p == NULL) {
@@ -1434,22 +1433,14 @@ static int garmin_attach(struct usb_serial *serial)
 }
 
 
-static void garmin_disconnect(struct usb_serial *serial)
+static int garmin_port_remove(struct usb_serial_port *port)
 {
-        struct usb_serial_port *port = serial->port[0];
         struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 
         usb_kill_urb(port->interrupt_in_urb);
         del_timer_sync(&garmin_data_p->timer);
-}
-
-
-static void garmin_release(struct usb_serial *serial)
-{
-        struct usb_serial_port *port = serial->port[0];
-        struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
-
         kfree(garmin_data_p);
+        return 0;
 }
 
 
@@ -1466,9 +1457,8 @@ static struct usb_serial_driver garmin_device = {
         .close               = garmin_close,
         .throttle            = garmin_throttle,
         .unthrottle          = garmin_unthrottle,
-        .attach              = garmin_attach,
-        .disconnect          = garmin_disconnect,
-        .release             = garmin_release,
+        .port_probe             = garmin_port_probe,
+        .port_remove            = garmin_port_remove,
         .write               = garmin_write,
         .write_room          = garmin_write_room,
         .write_bulk_callback = garmin_write_bulk_callback,
diff --git a/drivers/usb/serial/io_edgeport.c b/drivers/usb/serial/io_edgeport.c
index 8e6faaf3580c..5acc0d13864a 100644
--- a/drivers/usb/serial/io_edgeport.c
+++ b/drivers/usb/serial/io_edgeport.c
@@ -225,6 +225,8 @@ static int  edge_get_icount(struct tty_struct *tty,
 static int  edge_startup(struct usb_serial *serial);
 static void edge_disconnect(struct usb_serial *serial);
 static void edge_release(struct usb_serial *serial);
+static int edge_port_probe(struct usb_serial_port *port);
+static int edge_port_remove(struct usb_serial_port *port);
 
 #include "io_tables.h"  /* all of the devices that this driver supports */
 
@@ -2875,10 +2877,9 @@ static void load_application_firmware(struct edgeport_serial *edge_serial)
 static int edge_startup(struct usb_serial *serial)
 {
         struct edgeport_serial *edge_serial;
-        struct edgeport_port *edge_port;
         struct usb_device *dev;
         struct device *ddev = &serial->dev->dev;
-        int i, j;
+        int i;
         int response;
         bool interrupt_in_found;
         bool bulk_in_found;
@@ -2961,25 +2962,6 @@ static int edge_startup(struct usb_serial *serial)
         /* we set up the pointers to the endpoints in the edge_open function,
          * as the structures aren't created yet. */
 
-        /* set up our port private structures */
-        for (i = 0; i < serial->num_ports; ++i) {
-                edge_port = kzalloc(sizeof(struct edgeport_port), GFP_KERNEL);
-                if (edge_port == NULL) {
-                        dev_err(ddev, "%s - Out of memory\n", __func__);
-                        for (j = 0; j < i; ++j) {
-                                kfree(usb_get_serial_port_data(serial->port[j]));
-                                usb_set_serial_port_data(serial->port[j],
-                                                                        NULL);
-                        }
-                        usb_set_serial_data(serial, NULL);
-                        kfree(edge_serial);
-                        return -ENOMEM;
-                }
-                spin_lock_init(&edge_port->ep_lock);
-                edge_port->port = serial->port[i];
-                usb_set_serial_port_data(serial->port[i], edge_port);
-        }
-
         response = 0;
 
         if (edge_serial->is_epic) {
@@ -3120,14 +3102,36 @@ static void edge_disconnect(struct usb_serial *serial)
 static void edge_release(struct usb_serial *serial)
 {
         struct edgeport_serial *edge_serial = usb_get_serial_data(serial);
-        int i;
-
-        for (i = 0; i < serial->num_ports; ++i)
-                kfree(usb_get_serial_port_data(serial->port[i]));
 
         kfree(edge_serial);
 }
 
+static int edge_port_probe(struct usb_serial_port *port)
+{
+        struct edgeport_port *edge_port;
+
+        edge_port = kzalloc(sizeof(*edge_port), GFP_KERNEL);
+        if (!edge_port)
+                return -ENOMEM;
+
+        spin_lock_init(&edge_port->ep_lock);
+        edge_port->port = port;
+
+        usb_set_serial_port_data(port, edge_port);
+
+        return 0;
+}
+
+static int edge_port_remove(struct usb_serial_port *port)
+{
+        struct edgeport_port *edge_port;
+
+        edge_port = usb_get_serial_port_data(port);
+        kfree(edge_port);
+
+        return 0;
+}
+
 module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/io_tables.h b/drivers/usb/serial/io_tables.h
index 350afddb55ba..1511dd0ad324 100644
--- a/drivers/usb/serial/io_tables.h
+++ b/drivers/usb/serial/io_tables.h
@@ -110,6 +110,8 @@ static struct usb_serial_driver edgeport_2port_device = {
         .attach                 = edge_startup,
         .disconnect             = edge_disconnect,
         .release                = edge_release,
+        .port_probe             = edge_port_probe,
+        .port_remove            = edge_port_remove,
         .ioctl                  = edge_ioctl,
         .set_termios            = edge_set_termios,
         .tiocmget               = edge_tiocmget,
@@ -139,6 +141,8 @@ static struct usb_serial_driver edgeport_4port_device = {
         .attach                 = edge_startup,
         .disconnect             = edge_disconnect,
         .release                = edge_release,
+        .port_probe             = edge_port_probe,
+        .port_remove            = edge_port_remove,
         .ioctl                  = edge_ioctl,
         .set_termios            = edge_set_termios,
         .tiocmget               = edge_tiocmget,
@@ -168,6 +172,8 @@ static struct usb_serial_driver edgeport_8port_device = {
         .attach                 = edge_startup,
         .disconnect             = edge_disconnect,
         .release                = edge_release,
+        .port_probe             = edge_port_probe,
+        .port_remove            = edge_port_remove,
         .ioctl                  = edge_ioctl,
         .set_termios            = edge_set_termios,
         .tiocmget               = edge_tiocmget,
@@ -197,6 +203,8 @@ static struct usb_serial_driver epic_device = {
         .attach                 = edge_startup,
         .disconnect             = edge_disconnect,
         .release                = edge_release,
+        .port_probe             = edge_port_probe,
+        .port_remove            = edge_port_remove,
         .ioctl                  = edge_ioctl,
         .set_termios            = edge_set_termios,
         .tiocmget               = edge_tiocmget,
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index a2209cd45093..60023c2d2a31 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -2532,12 +2532,7 @@ static void edge_break(struct tty_struct *tty, int break_state)
 static int edge_startup(struct usb_serial *serial)
 {
         struct edgeport_serial *edge_serial;
-        struct edgeport_port *edge_port;
-        struct usb_device *dev;
         int status;
-        int i;
-
-        dev = serial->dev;
 
         /* create our private serial structure */
         edge_serial = kzalloc(sizeof(struct edgeport_serial), GFP_KERNEL);
@@ -2555,40 +2550,7 @@ static int edge_startup(struct usb_serial *serial)
                 return status;
         }
 
-        /* set up our port private structures */
-        for (i = 0; i < serial->num_ports; ++i) {
-                edge_port = kzalloc(sizeof(struct edgeport_port), GFP_KERNEL);
-                if (edge_port == NULL) {
-                        dev_err(&serial->dev->dev, "%s - Out of memory\n",
-                                                                __func__);
-                        goto cleanup;
-                }
-                spin_lock_init(&edge_port->ep_lock);
-                if (kfifo_alloc(&edge_port->write_fifo, EDGE_OUT_BUF_SIZE,
-                                                                GFP_KERNEL)) {
-                        dev_err(&serial->dev->dev, "%s - Out of memory\n",
-                                                                __func__);
-                        kfree(edge_port);
-                        goto cleanup;
-                }
-                edge_port->port = serial->port[i];
-                edge_port->edge_serial = edge_serial;
-                usb_set_serial_port_data(serial->port[i], edge_port);
-                edge_port->bUartMode = default_uart_mode;
-        }
-
         return 0;
-
-cleanup:
-        for (--i; i >= 0; --i) {
-                edge_port = usb_get_serial_port_data(serial->port[i]);
-                kfifo_free(&edge_port->write_fifo);
-                kfree(edge_port);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
-        kfree(edge_serial);
-        usb_set_serial_data(serial, NULL);
-        return -ENOMEM;
 }
 
 static void edge_disconnect(struct usb_serial *serial)
@@ -2597,17 +2559,54 @@ static void edge_disconnect(struct usb_serial *serial)
 
 static void edge_release(struct usb_serial *serial)
 {
-        int i;
+        kfree(usb_get_serial_data(serial));
+}
+
+static int edge_port_probe(struct usb_serial_port *port)
+{
         struct edgeport_port *edge_port;
+        int ret;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                edge_port = usb_get_serial_port_data(serial->port[i]);
+        edge_port = kzalloc(sizeof(*edge_port), GFP_KERNEL);
+        if (!edge_port)
+                return -ENOMEM;
+
+        ret = kfifo_alloc(&edge_port->write_fifo, EDGE_OUT_BUF_SIZE,
+                                                                GFP_KERNEL);
+        if (ret) {
+                kfree(edge_port);
+                return -ENOMEM;
+        }
+
+        spin_lock_init(&edge_port->ep_lock);
+        edge_port->port = port;
+        edge_port->edge_serial = usb_get_serial_data(port->serial);
+        edge_port->bUartMode = default_uart_mode;
+
+        usb_set_serial_port_data(port, edge_port);
+
+        ret = edge_create_sysfs_attrs(port);
+        if (ret) {
                 kfifo_free(&edge_port->write_fifo);
                 kfree(edge_port);
+                return ret;
         }
-        kfree(usb_get_serial_data(serial));
+
+        return 0;
 }
 
+static int edge_port_remove(struct usb_serial_port *port)
+{
+        struct edgeport_port *edge_port;
+
+        edge_port = usb_get_serial_port_data(port);
+
+        edge_remove_sysfs_attrs(port);
+        kfifo_free(&edge_port->write_fifo);
+        kfree(edge_port);
+
+        return 0;
+}
 
 /* Sysfs Attributes */
 
@@ -2667,8 +2666,8 @@ static struct usb_serial_driver edgeport_1port_device = {
         .attach                 = edge_startup,
         .disconnect             = edge_disconnect,
         .release                = edge_release,
-        .port_probe             = edge_create_sysfs_attrs,
-        .port_remove            = edge_remove_sysfs_attrs,
+        .port_probe             = edge_port_probe,
+        .port_remove            = edge_port_remove,
         .ioctl                  = edge_ioctl,
         .set_termios            = edge_set_termios,
         .tiocmget               = edge_tiocmget,
@@ -2698,8 +2697,8 @@ static struct usb_serial_driver edgeport_2port_device = {
         .attach                 = edge_startup,
         .disconnect             = edge_disconnect,
         .release                = edge_release,
-        .port_probe             = edge_create_sysfs_attrs,
-        .port_remove            = edge_remove_sysfs_attrs,
+        .port_probe             = edge_port_probe,
+        .port_remove            = edge_port_remove,
         .ioctl                  = edge_ioctl,
         .set_termios            = edge_set_termios,
         .tiocmget               = edge_tiocmget,
diff --git a/drivers/usb/serial/ipw.c b/drivers/usb/serial/ipw.c
index 20a132ec39e2..4264821a3b34 100644
--- a/drivers/usb/serial/ipw.c
+++ b/drivers/usb/serial/ipw.c
@@ -203,8 +203,7 @@ static int ipw_open(struct tty_struct *tty, struct usb_serial_port *port)
         return 0;
 }
 
-/* fake probe - only to allocate data structures */
-static int ipw_probe(struct usb_serial *serial, const struct usb_device_id *id)
+static int ipw_attach(struct usb_serial *serial)
 {
         struct usb_wwan_intf_private *data;
 
@@ -303,9 +302,9 @@ static struct usb_serial_driver ipw_device = {
         .num_ports =            1,
         .open =                 ipw_open,
         .close =                ipw_close,
-        .probe =                ipw_probe,
-        .attach =               usb_wwan_startup,
+        .attach =               ipw_attach,
         .release =              ipw_release,
+        .port_probe =           usb_wwan_port_probe,
         .port_remove =          usb_wwan_port_remove,
         .dtr_rts =              ipw_dtr_rts,
         .write =                usb_wwan_write,
diff --git a/drivers/usb/serial/iuu_phoenix.c b/drivers/usb/serial/iuu_phoenix.c
index 01da3ea36e89..cd5533e81de7 100644
--- a/drivers/usb/serial/iuu_phoenix.c
+++ b/drivers/usb/serial/iuu_phoenix.c
@@ -53,6 +53,8 @@ static int iuu_cardout;
 static bool xmas;
 static int vcc_default = 5;
 
+static int iuu_create_sysfs_attrs(struct usb_serial_port *port);
+static int iuu_remove_sysfs_attrs(struct usb_serial_port *port);
 static void read_rxcmd_callback(struct urb *urb);
 
 struct iuu_private {
@@ -72,63 +74,55 @@ struct iuu_private {
         u32 clk;
 };
 
-
-static void iuu_free_buf(struct iuu_private *priv)
-{
-        kfree(priv->buf);
-        kfree(priv->writebuf);
-}
-
-static int iuu_alloc_buf(struct usb_serial *serial, struct iuu_private *priv)
-{
-        priv->buf = kzalloc(256, GFP_KERNEL);
-        priv->writebuf = kzalloc(256, GFP_KERNEL);
-        if (!priv->buf || !priv->writebuf) {
-                iuu_free_buf(priv);
-                dev_dbg(&serial->dev->dev, "%s problem allocation buffer\n", __func__);
-                return -ENOMEM;
-        }
-        dev_dbg(&serial->dev->dev, "%s - Privates buffers allocation success\n", __func__);
-        return 0;
-}
-
-static int iuu_startup(struct usb_serial *serial)
+static int iuu_port_probe(struct usb_serial_port *port)
 {
         struct iuu_private *priv;
+        int ret;
 
         priv = kzalloc(sizeof(struct iuu_private), GFP_KERNEL);
-        dev_dbg(&serial->dev->dev, "%s- priv allocation success\n", __func__);
         if (!priv)
                 return -ENOMEM;
-        if (iuu_alloc_buf(serial, priv)) {
+
+        priv->buf = kzalloc(256, GFP_KERNEL);
+        if (!priv->buf) {
+                kfree(priv);
+                return -ENOMEM;
+        }
+
+        priv->writebuf = kzalloc(256, GFP_KERNEL);
+        if (!priv->writebuf) {
+                kfree(priv->buf);
                 kfree(priv);
                 return -ENOMEM;
         }
+
         priv->vcc = vcc_default;
         spin_lock_init(&priv->lock);
         init_waitqueue_head(&priv->delta_msr_wait);
-        usb_set_serial_port_data(serial->port[0], priv);
+
+        usb_set_serial_port_data(port, priv);
+
+        ret = iuu_create_sysfs_attrs(port);
+        if (ret) {
+                kfree(priv->writebuf);
+                kfree(priv->buf);
+                kfree(priv);
+                return ret;
+        }
+
         return 0;
 }
 
-/* Release function */
-static void iuu_release(struct usb_serial *serial)
+static int iuu_port_remove(struct usb_serial_port *port)
 {
-        struct usb_serial_port *port = serial->port[0];
         struct iuu_private *priv = usb_get_serial_port_data(port);
-        if (!port)
-                return;
 
-        if (priv) {
-                iuu_free_buf(priv);
-                dev_dbg(&port->dev, "%s - I will free all\n", __func__);
-                usb_set_serial_port_data(port, NULL);
-
-                dev_dbg(&port->dev, "%s - priv is not anymore in port structure\n", __func__);
-                kfree(priv);
+        iuu_remove_sysfs_attrs(port);
+        kfree(priv->writebuf);
+        kfree(priv->buf);
+        kfree(priv);
 
-                dev_dbg(&port->dev, "%s priv is now kfree\n", __func__);
-        }
+        return 0;
 }
 
 static int iuu_tiocmset(struct tty_struct *tty,
@@ -1215,8 +1209,6 @@ static struct usb_serial_driver iuu_device = {
         .num_ports = 1,
         .bulk_in_size = 512,
         .bulk_out_size = 512,
-        .port_probe = iuu_create_sysfs_attrs,
-        .port_remove = iuu_remove_sysfs_attrs,
         .open = iuu_open,
         .close = iuu_close,
         .write = iuu_uart_write,
@@ -1225,8 +1217,8 @@ static struct usb_serial_driver iuu_device = {
         .tiocmset = iuu_tiocmset,
         .set_termios = iuu_set_termios,
         .init_termios = iuu_init_termios,
-        .attach = iuu_startup,
-        .release = iuu_release,
+        .port_probe = iuu_port_probe,
+        .port_remove = iuu_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index 29c943d737d0..7179b0c5f814 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -1374,13 +1374,9 @@ static struct callbacks {
            data in device_details */
 static void keyspan_setup_urbs(struct usb_serial *serial)
 {
-        int                             i, j;
         struct keyspan_serial_private   *s_priv;
         const struct keyspan_device_details     *d_details;
-        struct usb_serial_port          *port;
-        struct keyspan_port_private     *p_priv;
         struct callbacks                *cback;
-        int                             endp;
 
         s_priv = usb_get_serial_data(serial);
         d_details = s_priv->device_details;
@@ -1404,45 +1400,6 @@ static void keyspan_setup_urbs(struct usb_serial *serial)
                 (serial, d_details->glocont_endpoint, USB_DIR_OUT,
                  serial, s_priv->glocont_buf, GLOCONT_BUFLEN,
                  cback->glocont_callback);
-
-        /* Setup endpoints for each port specific thing */
-        for (i = 0; i < d_details->num_ports; i++) {
-                port = serial->port[i];
-                p_priv = usb_get_serial_port_data(port);
-
-                /* Do indat endpoints first, once for each flip */
-                endp = d_details->indat_endpoints[i];
-                for (j = 0; j <= d_details->indat_endp_flip; ++j, ++endp) {
-                        p_priv->in_urbs[j] = keyspan_setup_urb
-                                (serial, endp, USB_DIR_IN, port,
-                                 p_priv->in_buffer[j], 64,
-                                 cback->indat_callback);
-                }
-                for (; j < 2; ++j)
-                        p_priv->in_urbs[j] = NULL;
-
-                /* outdat endpoints also have flip */
-                endp = d_details->outdat_endpoints[i];
-                for (j = 0; j <= d_details->outdat_endp_flip; ++j, ++endp) {
-                        p_priv->out_urbs[j] = keyspan_setup_urb
-                                (serial, endp, USB_DIR_OUT, port,
-                                 p_priv->out_buffer[j], 64,
-                                 cback->outdat_callback);
-                }
-                for (; j < 2; ++j)
-                        p_priv->out_urbs[j] = NULL;
-
-                /* inack endpoint */
-                p_priv->inack_urb = keyspan_setup_urb
-                        (serial, d_details->inack_endpoints[i], USB_DIR_IN,
-                         port, p_priv->inack_buffer, 1, cback->inack_callback);
-
-                /* outcont endpoint */
-                p_priv->outcont_urb = keyspan_setup_urb
-                        (serial, d_details->outcont_endpoints[i], USB_DIR_OUT,
-                         port, p_priv->outcont_buffer, 64,
-                         cback->outcont_callback);
-        }
 }
 
 /* usa19 function doesn't require prescaler */
@@ -2407,9 +2364,7 @@ static void keyspan_send_setup(struct usb_serial_port *port, int reset_port)
 static int keyspan_startup(struct usb_serial *serial)
 {
         int                             i, err;
-        struct usb_serial_port          *port;
         struct keyspan_serial_private   *s_priv;
-        struct keyspan_port_private     *p_priv;
         const struct keyspan_device_details     *d_details;
 
         for (i = 0; (d_details = keyspan_devices[i]) != NULL; ++i)
@@ -2432,19 +2387,6 @@ static int keyspan_startup(struct usb_serial *serial)
         s_priv->device_details = d_details;
         usb_set_serial_data(serial, s_priv);
 
-        /* Now setup per port private data */
-        for (i = 0; i < serial->num_ports; i++) {
-                port = serial->port[i];
-                p_priv = kzalloc(sizeof(struct keyspan_port_private),
-                                                                GFP_KERNEL);
-                if (!p_priv) {
-                        dev_dbg(&port->dev, "%s - kmalloc for keyspan_port_private (%d) failed!.\n", __func__, i);
-                        return 1;
-                }
-                p_priv->device_details = d_details;
-                usb_set_serial_port_data(port, p_priv);
-        }
-
         keyspan_setup_urbs(serial);
 
         if (s_priv->instat_urb != NULL) {
@@ -2463,59 +2405,112 @@ static int keyspan_startup(struct usb_serial *serial)
 
 static void keyspan_disconnect(struct usb_serial *serial)
 {
-        int                             i, j;
-        struct usb_serial_port          *port;
-        struct keyspan_serial_private   *s_priv;
-        struct keyspan_port_private     *p_priv;
+        struct keyspan_serial_private *s_priv;
 
         s_priv = usb_get_serial_data(serial);
 
-        /* Stop reading/writing urbs */
         stop_urb(s_priv->instat_urb);
         stop_urb(s_priv->glocont_urb);
         stop_urb(s_priv->indat_urb);
-        for (i = 0; i < serial->num_ports; ++i) {
-                port = serial->port[i];
-                p_priv = usb_get_serial_port_data(port);
-                stop_urb(p_priv->inack_urb);
-                stop_urb(p_priv->outcont_urb);
-                for (j = 0; j < 2; j++) {
-                        stop_urb(p_priv->in_urbs[j]);
-                        stop_urb(p_priv->out_urbs[j]);
-                }
-        }
+}
+
+static void keyspan_release(struct usb_serial *serial)
+{
+        struct keyspan_serial_private *s_priv;
+
+        s_priv = usb_get_serial_data(serial);
 
-        /* Now free them */
         usb_free_urb(s_priv->instat_urb);
         usb_free_urb(s_priv->indat_urb);
         usb_free_urb(s_priv->glocont_urb);
-        for (i = 0; i < serial->num_ports; ++i) {
-                port = serial->port[i];
-                p_priv = usb_get_serial_port_data(port);
-                usb_free_urb(p_priv->inack_urb);
-                usb_free_urb(p_priv->outcont_urb);
-                for (j = 0; j < 2; j++) {
-                        usb_free_urb(p_priv->in_urbs[j]);
-                        usb_free_urb(p_priv->out_urbs[j]);
-                }
-        }
+
+        kfree(s_priv);
 }
 
-static void keyspan_release(struct usb_serial *serial)
+static int keyspan_port_probe(struct usb_serial_port *port)
 {
-        int                             i;
-        struct usb_serial_port          *port;
-        struct keyspan_serial_private   *s_priv;
+        struct usb_serial *serial = port->serial;
+        struct keyspan_port_private *s_priv;
+        struct keyspan_port_private *p_priv;
+        const struct keyspan_device_details *d_details;
+        struct callbacks *cback;
+        int endp;
+        int port_num;
+        int i;
 
         s_priv = usb_get_serial_data(serial);
+        d_details = s_priv->device_details;
 
-        kfree(s_priv);
+        p_priv = kzalloc(sizeof(*p_priv), GFP_KERNEL);
+        if (!p_priv)
+                return -ENOMEM;
 
-        /* Now free per port private data */
-        for (i = 0; i < serial->num_ports; i++) {
-                port = serial->port[i];
-                kfree(usb_get_serial_port_data(port));
+        s_priv = usb_get_serial_data(port->serial);
+        p_priv->device_details = d_details;
+
+        /* Setup values for the various callback routines */
+        cback = &keyspan_callbacks[d_details->msg_format];
+
+        port_num = port->number - port->serial->minor;
+
+        /* Do indat endpoints first, once for each flip */
+        endp = d_details->indat_endpoints[port_num];
+        for (i = 0; i <= d_details->indat_endp_flip; ++i, ++endp) {
+                p_priv->in_urbs[i] = keyspan_setup_urb(serial, endp,
+                                                USB_DIR_IN, port,
+                                                p_priv->in_buffer[i], 64,
+                                                cback->indat_callback);
+        }
+        /* outdat endpoints also have flip */
+        endp = d_details->outdat_endpoints[port_num];
+        for (i = 0; i <= d_details->outdat_endp_flip; ++i, ++endp) {
+                p_priv->out_urbs[i] = keyspan_setup_urb(serial, endp,
+                                                USB_DIR_OUT, port,
+                                                p_priv->out_buffer[i], 64,
+                                                cback->outdat_callback);
+        }
+        /* inack endpoint */
+        p_priv->inack_urb = keyspan_setup_urb(serial,
+                                        d_details->inack_endpoints[port_num],
+                                        USB_DIR_IN, port,
+                                        p_priv->inack_buffer, 1,
+                                        cback->inack_callback);
+        /* outcont endpoint */
+        p_priv->outcont_urb = keyspan_setup_urb(serial,
+                                        d_details->outcont_endpoints[port_num],
+                                        USB_DIR_OUT, port,
+                                        p_priv->outcont_buffer, 64,
+                                         cback->outcont_callback);
+
+        usb_set_serial_port_data(port, p_priv);
+
+        return 0;
+}
+
+static int keyspan_port_remove(struct usb_serial_port *port)
+{
+        struct keyspan_port_private *p_priv;
+        int i;
+
+        p_priv = usb_get_serial_port_data(port);
+
+        stop_urb(p_priv->inack_urb);
+        stop_urb(p_priv->outcont_urb);
+        for (i = 0; i < 2; i++) {
+                stop_urb(p_priv->in_urbs[i]);
+                stop_urb(p_priv->out_urbs[i]);
+        }
+
+        usb_free_urb(p_priv->inack_urb);
+        usb_free_urb(p_priv->outcont_urb);
+        for (i = 0; i < 2; i++) {
+                usb_free_urb(p_priv->in_urbs[i]);
+                usb_free_urb(p_priv->out_urbs[i]);
         }
+
+        kfree(p_priv);
+
+        return 0;
 }
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/keyspan.h b/drivers/usb/serial/keyspan.h
index 0a8a40b5711e..0273dda303a4 100644
--- a/drivers/usb/serial/keyspan.h
+++ b/drivers/usb/serial/keyspan.h
@@ -42,6 +42,8 @@ static void keyspan_dtr_rts		(struct usb_serial_port *port, int on);
 static int  keyspan_startup             (struct usb_serial *serial);
 static void keyspan_disconnect          (struct usb_serial *serial);
 static void keyspan_release             (struct usb_serial *serial);
+static int keyspan_port_probe(struct usb_serial_port *port);
+static int keyspan_port_remove(struct usb_serial_port *port);
 static int  keyspan_write_room          (struct tty_struct *tty);
 
 static int  keyspan_write               (struct tty_struct *tty,
@@ -567,6 +569,8 @@ static struct usb_serial_driver keyspan_1port_device = {
         .attach                 = keyspan_startup,
         .disconnect             = keyspan_disconnect,
         .release                = keyspan_release,
+        .port_probe             = keyspan_port_probe,
+        .port_remove            = keyspan_port_remove,
 };
 
 static struct usb_serial_driver keyspan_2port_device = {
@@ -589,6 +593,8 @@ static struct usb_serial_driver keyspan_2port_device = {
         .attach                 = keyspan_startup,
         .disconnect             = keyspan_disconnect,
         .release                = keyspan_release,
+        .port_probe             = keyspan_port_probe,
+        .port_remove            = keyspan_port_remove,
 };
 
 static struct usb_serial_driver keyspan_4port_device = {
@@ -611,6 +617,8 @@ static struct usb_serial_driver keyspan_4port_device = {
         .attach                 = keyspan_startup,
         .disconnect             = keyspan_disconnect,
         .release                = keyspan_release,
+        .port_probe             = keyspan_port_probe,
+        .port_remove            = keyspan_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index ca43ecb4a2bd..bb87e29c4ac2 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -713,29 +713,33 @@ MODULE_FIRMWARE("keyspan_pda/keyspan_pda.fw");
 MODULE_FIRMWARE("keyspan_pda/xircom_pgs.fw");
 #endif
 
-static int keyspan_pda_startup(struct usb_serial *serial)
+static int keyspan_pda_port_probe(struct usb_serial_port *port)
 {
 
         struct keyspan_pda_private *priv;
 
-        /* allocate the private data structures for all ports. Well, for all
-           one ports. */
-
         priv = kmalloc(sizeof(struct keyspan_pda_private), GFP_KERNEL);
         if (!priv)
-                return 1; /* error */
-        usb_set_serial_port_data(serial->port[0], priv);
-        init_waitqueue_head(&serial->port[0]->write_wait);
+                return -ENOMEM;
+
         INIT_WORK(&priv->wakeup_work, keyspan_pda_wakeup_write);
         INIT_WORK(&priv->unthrottle_work, keyspan_pda_request_unthrottle);
-        priv->serial = serial;
-        priv->port = serial->port[0];
+        priv->serial = port->serial;
+        priv->port = port;
+
+        usb_set_serial_port_data(port, priv);
+
         return 0;
 }
 
-static void keyspan_pda_release(struct usb_serial *serial)
+static int keyspan_pda_port_remove(struct usb_serial_port *port)
 {
-        kfree(usb_get_serial_port_data(serial->port[0]));
+        struct keyspan_pda_private *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 #ifdef KEYSPAN
@@ -786,8 +790,8 @@ static struct usb_serial_driver keyspan_pda_device = {
         .break_ctl =            keyspan_pda_break_ctl,
         .tiocmget =             keyspan_pda_tiocmget,
         .tiocmset =             keyspan_pda_tiocmset,
-        .attach =               keyspan_pda_startup,
-        .release =              keyspan_pda_release,
+        .port_probe =           keyspan_pda_port_probe,
+        .port_remove =          keyspan_pda_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/kl5kusb105.c b/drivers/usb/serial/kl5kusb105.c
index 3f6d7376c02d..1f4517864cd2 100644
--- a/drivers/usb/serial/kl5kusb105.c
+++ b/drivers/usb/serial/kl5kusb105.c
@@ -60,8 +60,8 @@
 /*
  * Function prototypes
  */
-static int  klsi_105_startup(struct usb_serial *serial);
-static void klsi_105_release(struct usb_serial *serial);
+static int klsi_105_port_probe(struct usb_serial_port *port);
+static int klsi_105_port_remove(struct usb_serial_port *port);
 static int  klsi_105_open(struct tty_struct *tty, struct usb_serial_port *port);
 static void klsi_105_close(struct usb_serial_port *port);
 static void klsi_105_set_termios(struct tty_struct *tty,
@@ -99,8 +99,8 @@ static struct usb_serial_driver kl5kusb105d_device = {
         /*.break_ctl =          klsi_105_break_ctl,*/
         .tiocmget =             klsi_105_tiocmget,
         .tiocmset =             klsi_105_tiocmset,
-        .attach =               klsi_105_startup,
-        .release =              klsi_105_release,
+        .port_probe =           klsi_105_port_probe,
+        .port_remove =          klsi_105_port_remove,
         .throttle =             usb_serial_generic_throttle,
         .unthrottle =           usb_serial_generic_unthrottle,
         .process_read_urb =     klsi_105_process_read_urb,
@@ -223,60 +223,40 @@ static int klsi_105_get_line_state(struct usb_serial_port *port,
  * Driver's tty interface functions
  */
 
-static int klsi_105_startup(struct usb_serial *serial)
+static int klsi_105_port_probe(struct usb_serial_port *port)
 {
         struct klsi_105_private *priv;
-        int i;
 
-        /* check if we support the product id (see keyspan.c)
-         * FIXME
-         */
+        priv = kmalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
 
-        /* allocate the private data structure */
-        for (i = 0; i < serial->num_ports; i++) {
-                priv = kmalloc(sizeof(struct klsi_105_private),
-                                                   GFP_KERNEL);
-                if (!priv) {
-                        dev_dbg(&serial->interface->dev,
-                                "%s - kmalloc for klsi_105_private failed.\n",
-                                __func__);
-                        i--;
-                        goto err_cleanup;
-                }
-                /* set initial values for control structures */
-                priv->cfg.pktlen    = 5;
-                priv->cfg.baudrate  = kl5kusb105a_sio_b9600;
-                priv->cfg.databits  = kl5kusb105a_dtb_8;
-                priv->cfg.unknown1  = 0;
-                priv->cfg.unknown2  = 1;
+        /* set initial values for control structures */
+        priv->cfg.pktlen    = 5;
+        priv->cfg.baudrate  = kl5kusb105a_sio_b9600;
+        priv->cfg.databits  = kl5kusb105a_dtb_8;
+        priv->cfg.unknown1  = 0;
+        priv->cfg.unknown2  = 1;
 
-                priv->line_state    = 0;
+        priv->line_state    = 0;
 
-                usb_set_serial_port_data(serial->port[i], priv);
+        spin_lock_init(&priv->lock);
 
-                spin_lock_init(&priv->lock);
+        /* priv->termios is left uninitialized until port opening */
 
-                /* priv->termios is left uninitialized until port opening */
-                init_waitqueue_head(&serial->port[i]->write_wait);
-        }
+        usb_set_serial_port_data(port, priv);
 
         return 0;
-
-err_cleanup:
-        for (; i >= 0; i--) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
-        return -ENOMEM;
 }
 
-static void klsi_105_release(struct usb_serial *serial)
+static int klsi_105_port_remove(struct usb_serial_port *port)
 {
-        int i;
+        struct klsi_105_private *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
 
-        for (i = 0; i < serial->num_ports; ++i)
-                kfree(usb_get_serial_port_data(serial->port[i]));
+        return 0;
 }
 
 static int  klsi_105_open(struct tty_struct *tty, struct usb_serial_port *port)
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index 5c4d2fbd4e11..c9ca7a5b12e0 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -54,8 +54,8 @@
 
 
 /* Function prototypes */
-static int  kobil_startup(struct usb_serial *serial);
-static void kobil_release(struct usb_serial *serial);
+static int kobil_port_probe(struct usb_serial_port *probe);
+static int kobil_port_remove(struct usb_serial_port *probe);
 static int  kobil_open(struct tty_struct *tty, struct usb_serial_port *port);
 static void kobil_close(struct usb_serial_port *port);
 static int  kobil_write(struct tty_struct *tty, struct usb_serial_port *port,
@@ -89,8 +89,8 @@ static struct usb_serial_driver kobil_device = {
         .description =          "KOBIL USB smart card terminal",
         .id_table =             id_table,
         .num_ports =            1,
-        .attach =               kobil_startup,
-        .release =              kobil_release,
+        .port_probe =           kobil_port_probe,
+        .port_remove =          kobil_port_remove,
         .ioctl =                kobil_ioctl,
         .set_termios =          kobil_set_termios,
         .init_termios =         kobil_init_termios,
@@ -117,9 +117,10 @@ struct kobil_private {
 };
 
 
-static int kobil_startup(struct usb_serial *serial)
+static int kobil_port_probe(struct usb_serial_port *port)
 {
         int i;
+        struct usb_serial *serial = port->serial;
         struct kobil_private *priv;
         struct usb_device *pdev;
         struct usb_host_config *actconfig;
@@ -149,7 +150,7 @@ static int kobil_startup(struct usb_serial *serial)
                 dev_dbg(&serial->dev->dev, "KOBIL KAAN SIM detected\n");
                 break;
         }
-        usb_set_serial_port_data(serial->port[0], priv);
+        usb_set_serial_port_data(port, priv);
 
         /* search for the necessary endpoints */
         pdev = serial->dev;
@@ -179,12 +180,14 @@ static int kobil_startup(struct usb_serial *serial)
 }
 
 
-static void kobil_release(struct usb_serial *serial)
+static int kobil_port_remove(struct usb_serial_port *port)
 {
-        int i;
+        struct kobil_private *priv;
 
-        for (i = 0; i < serial->num_ports; ++i)
-                kfree(usb_get_serial_port_data(serial->port[i]));
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 static void kobil_init_termios(struct tty_struct *tty)
diff --git a/drivers/usb/serial/mct_u232.c b/drivers/usb/serial/mct_u232.c
index f3947712e137..8a2081004107 100644
--- a/drivers/usb/serial/mct_u232.c
+++ b/drivers/usb/serial/mct_u232.c
@@ -49,7 +49,8 @@
  * Function prototypes
  */
 static int  mct_u232_startup(struct usb_serial *serial);
-static void mct_u232_release(struct usb_serial *serial);
+static int  mct_u232_port_probe(struct usb_serial_port *port);
+static int  mct_u232_port_remove(struct usb_serial_port *remove);
 static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port);
 static void mct_u232_close(struct usb_serial_port *port);
 static void mct_u232_dtr_rts(struct usb_serial_port *port, int on);
@@ -99,7 +100,8 @@ static struct usb_serial_driver mct_u232_device = {
         .tiocmget =          mct_u232_tiocmget,
         .tiocmset =          mct_u232_tiocmset,
         .attach =            mct_u232_startup,
-        .release =           mct_u232_release,
+        .port_probe =        mct_u232_port_probe,
+        .port_remove =       mct_u232_port_remove,
         .ioctl =             mct_u232_ioctl,
         .get_icount =        mct_u232_get_icount,
 };
@@ -388,18 +390,8 @@ static void mct_u232_msr_to_state(struct usb_serial_port *port,
 
 static int mct_u232_startup(struct usb_serial *serial)
 {
-        struct mct_u232_private *priv;
         struct usb_serial_port *port, *rport;
 
-        priv = kzalloc(sizeof(struct mct_u232_private), GFP_KERNEL);
-        if (!priv)
-                return -ENOMEM;
-        spin_lock_init(&priv->lock);
-        init_waitqueue_head(&priv->msr_wait);
-        usb_set_serial_port_data(serial->port[0], priv);
-
-        init_waitqueue_head(&serial->port[0]->write_wait);
-
         /* Puh, that's dirty */
         port = serial->port[0];
         rport = serial->port[1];
@@ -412,18 +404,31 @@ static int mct_u232_startup(struct usb_serial *serial)
         return 0;
 } /* mct_u232_startup */
 
+static int mct_u232_port_probe(struct usb_serial_port *port)
+{
+        struct mct_u232_private *priv;
+
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        spin_lock_init(&priv->lock);
+        init_waitqueue_head(&priv->msr_wait);
+
+        usb_set_serial_port_data(port, priv);
 
-static void mct_u232_release(struct usb_serial *serial)
+        return 0;
+}
+
+static int mct_u232_port_remove(struct usb_serial_port *port)
 {
         struct mct_u232_private *priv;
-        int i;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                /* My special items, the standard routines free my urbs */
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-        }
-} /* mct_u232_release */
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
+}
 
 static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
@@ -515,12 +520,14 @@ static void mct_u232_dtr_rts(struct usb_serial_port *port, int on)
 
 static void mct_u232_close(struct usb_serial_port *port)
 {
-        if (port->serial->dev) {
-                /* shutdown our urbs */
-                usb_kill_urb(port->write_urb);
-                usb_kill_urb(port->read_urb);
-                usb_kill_urb(port->interrupt_in_urb);
-        }
+        /*
+         * Must kill the read urb as it is actually an interrupt urb, which
+         * generic close thus fails to kill.
+         */
+        usb_kill_urb(port->read_urb);
+        usb_kill_urb(port->interrupt_in_urb);
+
+        usb_serial_generic_close(port);
 } /* mct_u232_close */
 
 
diff --git a/drivers/usb/serial/metro-usb.c b/drivers/usb/serial/metro-usb.c
index 0b257ddffbdb..6f29c74eb769 100644
--- a/drivers/usb/serial/metro-usb.c
+++ b/drivers/usb/serial/metro-usb.c
@@ -179,16 +179,13 @@ static void metrousb_cleanup(struct usb_serial_port *port)
 {
         dev_dbg(&port->dev, "%s\n", __func__);
 
-        if (port->serial->dev) {
-                /* Shutdown any interrupt in urbs. */
-                if (port->interrupt_in_urb) {
-                        usb_unlink_urb(port->interrupt_in_urb);
-                        usb_kill_urb(port->interrupt_in_urb);
-                }
-
-                /* Send deactivate cmd to device */
+        usb_unlink_urb(port->interrupt_in_urb);
+        usb_kill_urb(port->interrupt_in_urb);
+
+        mutex_lock(&port->serial->disc_mutex);
+        if (!port->serial->disconnected)
                 metrousb_send_unidirectional_cmd(UNI_CMD_CLOSE, port);
-        }
+        mutex_unlock(&port->serial->disc_mutex);
 }
 
 static int metrousb_open(struct tty_struct *tty, struct usb_serial_port *port)
@@ -271,51 +268,27 @@ static int metrousb_set_modem_ctrl(struct usb_serial *serial, unsigned int contr
         return retval;
 }
 
-static void metrousb_shutdown(struct usb_serial *serial)
+static int metrousb_port_probe(struct usb_serial_port *port)
 {
-        int i = 0;
+        struct metrousb_private *metro_priv;
 
-        dev_dbg(&serial->dev->dev, "%s\n", __func__);
+        metro_priv = kzalloc(sizeof(*metro_priv), GFP_KERNEL);
+        if (!metro_priv)
+                return -ENOMEM;
 
-        /* Stop reading and writing on all ports. */
-        for (i = 0; i < serial->num_ports; ++i) {
-                /* Close any open urbs. */
-                metrousb_cleanup(serial->port[i]);
+        spin_lock_init(&metro_priv->lock);
 
-                /* Free memory. */
-                kfree(usb_get_serial_port_data(serial->port[i]));
-                usb_set_serial_port_data(serial->port[i], NULL);
+        usb_set_serial_port_data(port, metro_priv);
 
-                dev_dbg(&serial->dev->dev, "%s - freed port number=%d\n",
-                        __func__, serial->port[i]->number);
-        }
+        return 0;
 }
 
-static int metrousb_startup(struct usb_serial *serial)
+static int metrousb_port_remove(struct usb_serial_port *port)
 {
         struct metrousb_private *metro_priv;
-        struct usb_serial_port *port;
-        int i = 0;
 
-        dev_dbg(&serial->dev->dev, "%s\n", __func__);
-
-        /* Loop through the serial ports setting up the private structures.
-         * Currently we only use one port. */
-        for (i = 0; i < serial->num_ports; ++i) {
-                port = serial->port[i];
-
-                /* Declare memory. */
-                metro_priv = kzalloc(sizeof(struct metrousb_private), GFP_KERNEL);
-                if (!metro_priv)
-                        return -ENOMEM;
-
-                /* Initialize memory. */
-                spin_lock_init(&metro_priv->lock);
-                usb_set_serial_port_data(port, metro_priv);
-
-                dev_dbg(&serial->dev->dev, "%s - port number=%d\n ",
-                        __func__, port->number);
-        }
+        metro_priv = usb_get_serial_port_data(port);
+        kfree(metro_priv);
 
         return 0;
 }
@@ -414,8 +387,8 @@ static struct usb_serial_driver metrousb_device = {
         .close                  = metrousb_cleanup,
         .read_int_callback      = metrousb_read_int_callback,
         .write_int_callback     = metrousb_write_int_callback,
-        .attach                 = metrousb_startup,
-        .release                = metrousb_shutdown,
+        .port_probe             = metrousb_port_probe,
+        .port_remove            = metrousb_port_remove,
         .throttle               = metrousb_throttle,
         .unthrottle             = metrousb_unthrottle,
         .tiocmget               = metrousb_tiocmget,
diff --git a/drivers/usb/serial/mos7720.c b/drivers/usb/serial/mos7720.c
index 1bf1ad066666..75267421aad8 100644
--- a/drivers/usb/serial/mos7720.c
+++ b/drivers/usb/serial/mos7720.c
@@ -1966,9 +1966,7 @@ static int mos7720_ioctl(struct tty_struct *tty,
 
 static int mos7720_startup(struct usb_serial *serial)
 {
-        struct moschip_port *mos7720_port;
         struct usb_device *dev;
-        int i;
         char data;
         u16 product;
         int ret_val;
@@ -1999,29 +1997,6 @@ static int mos7720_startup(struct usb_serial *serial)
                 serial->port[1]->interrupt_in_buffer = NULL;
         }
 
-
-        /* set up serial port private structures */
-        for (i = 0; i < serial->num_ports; ++i) {
-                mos7720_port = kzalloc(sizeof(struct moschip_port), GFP_KERNEL);
-                if (mos7720_port == NULL) {
-                        dev_err(&dev->dev, "%s - Out of memory\n", __func__);
-                        return -ENOMEM;
-                }
-
-                /* Initialize all port interrupt end point to port 0 int
-                 * endpoint.  Our device has only one interrupt endpoint
-                 * common to all ports */
-                serial->port[i]->interrupt_in_endpointAddress =
-                                serial->port[0]->interrupt_in_endpointAddress;
-
-                mos7720_port->port = serial->port[i];
-                usb_set_serial_port_data(serial->port[i], mos7720_port);
-
-                dev_dbg(&dev->dev, "port number is %d\n", serial->port[i]->number);
-                dev_dbg(&dev->dev, "serial number is %d\n", serial->minor);
-        }
-
-
         /* setting configuration feature to one */
         usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
                         (__u8)0x03, 0x00, 0x01, 0x00, NULL, 0x00, 5*HZ);
@@ -2049,8 +2024,6 @@ static int mos7720_startup(struct usb_serial *serial)
 
 static void mos7720_release(struct usb_serial *serial)
 {
-        int i;
-
 #ifdef CONFIG_USB_SERIAL_MOS7715_PARPORT
         /* close the parallel port */
 
@@ -2089,9 +2062,36 @@ static void mos7720_release(struct usb_serial *serial)
                 kref_put(&mos_parport->ref_count, destroy_mos_parport);
         }
 #endif
-        /* free private structure allocated for serial port */
-        for (i = 0; i < serial->num_ports; ++i)
-                kfree(usb_get_serial_port_data(serial->port[i]));
+}
+
+static int mos7720_port_probe(struct usb_serial_port *port)
+{
+        struct moschip_port *mos7720_port;
+
+        mos7720_port = kzalloc(sizeof(*mos7720_port), GFP_KERNEL);
+        if (!mos7720_port)
+                return -ENOMEM;
+
+        /* Initialize all port interrupt end point to port 0 int endpoint.
+         * Our device has only one interrupt endpoint common to all ports.
+         */
+        port->interrupt_in_endpointAddress =
+                port->serial->port[0]->interrupt_in_endpointAddress;
+        mos7720_port->port = port;
+
+        usb_set_serial_port_data(port, mos7720_port);
+
+        return 0;
+}
+
+static int mos7720_port_remove(struct usb_serial_port *port)
+{
+        struct moschip_port *mos7720_port;
+
+        mos7720_port = usb_get_serial_port_data(port);
+        kfree(mos7720_port);
+
+        return 0;
 }
 
 static struct usb_serial_driver moschip7720_2port_driver = {
@@ -2109,6 +2109,8 @@ static struct usb_serial_driver moschip7720_2port_driver = {
         .probe                  = mos77xx_probe,
         .attach                 = mos7720_startup,
         .release                = mos7720_release,
+        .port_probe             = mos7720_port_probe,
+        .port_remove            = mos7720_port_remove,
         .ioctl                  = mos7720_ioctl,
         .tiocmget               = mos7720_tiocmget,
         .tiocmset               = mos7720_tiocmset,
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index d6d4eeca8c68..1cf3375ec1af 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -218,12 +218,10 @@ struct moschip_port {
         int port_num;           /*Actual port number in the device(1,2,etc) */
         struct urb *write_urb;  /* write URB for this port */
         struct urb *read_urb;   /* read URB for this port */
-        struct urb *int_urb;
         __u8 shadowLCR;         /* last LCR value received */
         __u8 shadowMCR;         /* last MCR value received */
         char open;
         char open_ports;
-        char zombie;
         wait_queue_head_t wait_chase;   /* for handling sleeping while waiting for chase to finish */
         wait_queue_head_t delta_msr_wait;       /* for handling sleeping while waiting for msr change to happen */
         int delta_msr_cond;
@@ -478,7 +476,6 @@ static void mos7840_control_callback(struct urb *urb)
         struct moschip_port *mos7840_port;
         struct device *dev = &urb->dev->dev;
         __u8 regval = 0x0;
-        int result = 0;
         int status = urb->status;
 
         mos7840_port = urb->context;
@@ -495,7 +492,7 @@ static void mos7840_control_callback(struct urb *urb)
                 return;
         default:
                 dev_dbg(dev, "%s - nonzero urb status received: %d\n", __func__, status);
-                goto exit;
+                return;
         }
 
         dev_dbg(dev, "%s urb buffer size is %d\n", __func__, urb->actual_length);
@@ -508,16 +505,6 @@ static void mos7840_control_callback(struct urb *urb)
                 mos7840_handle_new_msr(mos7840_port, regval);
         else if (mos7840_port->MsrLsr == 1)
                 mos7840_handle_new_lsr(mos7840_port, regval);
-
-exit:
-        spin_lock(&mos7840_port->pool_lock);
-        if (!mos7840_port->zombie)
-                result = usb_submit_urb(mos7840_port->int_urb, GFP_ATOMIC);
-        spin_unlock(&mos7840_port->pool_lock);
-        if (result) {
-                dev_err(dev, "%s - Error %d submitting interrupt urb\n",
-                        __func__, result);
-        }
 }
 
 static int mos7840_get_reg(struct moschip_port *mcs, __u16 Wval, __u16 reg,
@@ -686,14 +673,7 @@ static void mos7840_interrupt_callback(struct urb *urb)
                                         wreg = MODEM_STATUS_REGISTER;
                                         break;
                                 }
-                                spin_lock(&mos7840_port->pool_lock);
-                                if (!mos7840_port->zombie) {
-                                        rv = mos7840_get_reg(mos7840_port, wval, wreg, &Data);
-                                } else {
-                                        spin_unlock(&mos7840_port->pool_lock);
-                                        return;
-                                }
-                                spin_unlock(&mos7840_port->pool_lock);
+                                rv = mos7840_get_reg(mos7840_port, wval, wreg, &Data);
                         }
                 }
         }
@@ -2347,309 +2327,249 @@ static int mos7840_calc_num_ports(struct usb_serial *serial)
         return mos7840_num_ports;
 }
 
-/****************************************************************************
- * mos7840_startup
- ****************************************************************************/
-
-static int mos7840_startup(struct usb_serial *serial)
+static int mos7840_port_probe(struct usb_serial_port *port)
 {
+        struct usb_serial *serial = port->serial;
         struct moschip_port *mos7840_port;
-        struct usb_device *dev;
-        int i, status;
+        int status;
+        int pnum;
         __u16 Data;
 
-        dev = serial->dev;
-
         /* we set up the pointers to the endpoints in the mos7840_open *
          * function, as the structures aren't created yet.             */
 
-        /* set up port private structures */
-        for (i = 0; i < serial->num_ports; ++i) {
-                dev_dbg(&dev->dev, "mos7840_startup: configuring port %d............\n", i);
-                mos7840_port = kzalloc(sizeof(struct moschip_port), GFP_KERNEL);
-                if (mos7840_port == NULL) {
-                        dev_err(&dev->dev, "%s - Out of memory\n", __func__);
-                        status = -ENOMEM;
-                        i--; /* don't follow NULL pointer cleaning up */
-                        goto error;
-                }
-
-                /* Initialize all port interrupt end point to port 0 int
-                 * endpoint. Our device has only one interrupt end point
-                 * common to all port */
-
-                mos7840_port->port = serial->port[i];
-                mos7840_set_port_private(serial->port[i], mos7840_port);
-                spin_lock_init(&mos7840_port->pool_lock);
-
-                /* minor is not initialised until later by
-                 * usb-serial.c:get_free_serial() and cannot therefore be used
-                 * to index device instances */
-                mos7840_port->port_num = i + 1;
-                dev_dbg(&dev->dev, "serial->port[i]->number = %d\n", serial->port[i]->number);
-                dev_dbg(&dev->dev, "serial->port[i]->serial->minor = %d\n", serial->port[i]->serial->minor);
-                dev_dbg(&dev->dev, "mos7840_port->port_num = %d\n", mos7840_port->port_num);
-                dev_dbg(&dev->dev, "serial->minor = %d\n", serial->minor);
-
-                if (mos7840_port->port_num == 1) {
-                        mos7840_port->SpRegOffset = 0x0;
-                        mos7840_port->ControlRegOffset = 0x1;
-                        mos7840_port->DcrRegOffset = 0x4;
-                } else if ((mos7840_port->port_num == 2)
-                           && (serial->num_ports == 4)) {
-                        mos7840_port->SpRegOffset = 0x8;
-                        mos7840_port->ControlRegOffset = 0x9;
-                        mos7840_port->DcrRegOffset = 0x16;
-                } else if ((mos7840_port->port_num == 2)
-                           && (serial->num_ports == 2)) {
-                        mos7840_port->SpRegOffset = 0xa;
-                        mos7840_port->ControlRegOffset = 0xb;
-                        mos7840_port->DcrRegOffset = 0x19;
-                } else if ((mos7840_port->port_num == 3)
-                           && (serial->num_ports == 4)) {
-                        mos7840_port->SpRegOffset = 0xa;
-                        mos7840_port->ControlRegOffset = 0xb;
-                        mos7840_port->DcrRegOffset = 0x19;
-                } else if ((mos7840_port->port_num == 4)
-                           && (serial->num_ports == 4)) {
-                        mos7840_port->SpRegOffset = 0xc;
-                        mos7840_port->ControlRegOffset = 0xd;
-                        mos7840_port->DcrRegOffset = 0x1c;
-                }
-                mos7840_dump_serial_port(serial->port[i], mos7840_port);
-                mos7840_set_port_private(serial->port[i], mos7840_port);
+        pnum = port->number - serial->minor;
 
-                /* enable rx_disable bit in control register */
-                status = mos7840_get_reg_sync(serial->port[i],
-                                 mos7840_port->ControlRegOffset, &Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Reading ControlReg failed status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "ControlReg Reading success val is %x, status%d\n", Data, status);
-                Data |= 0x08;   /* setting driver done bit */
-                Data |= 0x04;   /* sp1_bit to have cts change reflect in
-                                   modem status reg */
-
-                /* Data |= 0x20; //rx_disable bit */
-                status = mos7840_set_reg_sync(serial->port[i],
-                                         mos7840_port->ControlRegOffset, Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing ControlReg failed(rx_disable) status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "ControlReg Writing success(rx_disable) status%d\n", status);
+        dev_dbg(&port->dev, "mos7840_startup: configuring port %d\n", pnum);
+        mos7840_port = kzalloc(sizeof(struct moschip_port), GFP_KERNEL);
+        if (mos7840_port == NULL) {
+                dev_err(&port->dev, "%s - Out of memory\n", __func__);
+                return -ENOMEM;
+        }
 
-                /* Write default values in DCR (i.e 0x01 in DCR0, 0x05 in DCR2
-                   and 0x24 in DCR3 */
-                Data = 0x01;
-                status = mos7840_set_reg_sync(serial->port[i],
-                         (__u16) (mos7840_port->DcrRegOffset + 0), Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing DCR0 failed status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "DCR0 Writing success status%d\n", status);
+        /* Initialize all port interrupt end point to port 0 int
+         * endpoint. Our device has only one interrupt end point
+         * common to all port */
+
+        mos7840_port->port = port;
+        mos7840_set_port_private(port, mos7840_port);
+        spin_lock_init(&mos7840_port->pool_lock);
+
+        /* minor is not initialised until later by
+         * usb-serial.c:get_free_serial() and cannot therefore be used
+         * to index device instances */
+        mos7840_port->port_num = pnum + 1;
+        dev_dbg(&port->dev, "port->number = %d\n", port->number);
+        dev_dbg(&port->dev, "port->serial->minor = %d\n", port->serial->minor);
+        dev_dbg(&port->dev, "mos7840_port->port_num = %d\n", mos7840_port->port_num);
+        dev_dbg(&port->dev, "serial->minor = %d\n", serial->minor);
+
+        if (mos7840_port->port_num == 1) {
+                mos7840_port->SpRegOffset = 0x0;
+                mos7840_port->ControlRegOffset = 0x1;
+                mos7840_port->DcrRegOffset = 0x4;
+        } else if ((mos7840_port->port_num == 2) && (serial->num_ports == 4)) {
+                mos7840_port->SpRegOffset = 0x8;
+                mos7840_port->ControlRegOffset = 0x9;
+                mos7840_port->DcrRegOffset = 0x16;
+        } else if ((mos7840_port->port_num == 2) && (serial->num_ports == 2)) {
+                mos7840_port->SpRegOffset = 0xa;
+                mos7840_port->ControlRegOffset = 0xb;
+                mos7840_port->DcrRegOffset = 0x19;
+        } else if ((mos7840_port->port_num == 3) && (serial->num_ports == 4)) {
+                mos7840_port->SpRegOffset = 0xa;
+                mos7840_port->ControlRegOffset = 0xb;
+                mos7840_port->DcrRegOffset = 0x19;
+        } else if ((mos7840_port->port_num == 4) && (serial->num_ports == 4)) {
+                mos7840_port->SpRegOffset = 0xc;
+                mos7840_port->ControlRegOffset = 0xd;
+                mos7840_port->DcrRegOffset = 0x1c;
+        }
+        mos7840_dump_serial_port(port, mos7840_port);
+        mos7840_set_port_private(port, mos7840_port);
+
+        /* enable rx_disable bit in control register */
+        status = mos7840_get_reg_sync(port,
+                        mos7840_port->ControlRegOffset, &Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Reading ControlReg failed status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "ControlReg Reading success val is %x, status%d\n", Data, status);
+        Data |= 0x08;   /* setting driver done bit */
+        Data |= 0x04;   /* sp1_bit to have cts change reflect in
+                           modem status reg */
 
-                Data = 0x05;
-                status = mos7840_set_reg_sync(serial->port[i],
-                         (__u16) (mos7840_port->DcrRegOffset + 1), Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing DCR1 failed status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "DCR1 Writing success status%d\n", status);
+        /* Data |= 0x20; //rx_disable bit */
+        status = mos7840_set_reg_sync(port,
+                        mos7840_port->ControlRegOffset, Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing ControlReg failed(rx_disable) status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "ControlReg Writing success(rx_disable) status%d\n", status);
 
-                Data = 0x24;
-                status = mos7840_set_reg_sync(serial->port[i],
-                         (__u16) (mos7840_port->DcrRegOffset + 2), Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing DCR2 failed status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "DCR2 Writing success status%d\n", status);
+        /* Write default values in DCR (i.e 0x01 in DCR0, 0x05 in DCR2
+           and 0x24 in DCR3 */
+        Data = 0x01;
+        status = mos7840_set_reg_sync(port,
+                        (__u16) (mos7840_port->DcrRegOffset + 0), Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing DCR0 failed status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "DCR0 Writing success status%d\n", status);
 
-                /* write values in clkstart0x0 and clkmulti 0x20 */
-                Data = 0x0;
-                status = mos7840_set_reg_sync(serial->port[i],
-                                         CLK_START_VALUE_REGISTER, Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing CLK_START_VALUE_REGISTER failed status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "CLK_START_VALUE_REGISTER Writing success status%d\n", status);
+        Data = 0x05;
+        status = mos7840_set_reg_sync(port,
+                        (__u16) (mos7840_port->DcrRegOffset + 1), Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing DCR1 failed status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "DCR1 Writing success status%d\n", status);
 
-                Data = 0x20;
-                status = mos7840_set_reg_sync(serial->port[i],
-                                        CLK_MULTI_REGISTER, Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing CLK_MULTI_REGISTER failed status-0x%x\n", status);
-                        goto error;
-                } else
-                        dev_dbg(&dev->dev, "CLK_MULTI_REGISTER Writing success status%d\n", status);
+        Data = 0x24;
+        status = mos7840_set_reg_sync(port,
+                        (__u16) (mos7840_port->DcrRegOffset + 2), Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing DCR2 failed status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "DCR2 Writing success status%d\n", status);
 
-                /* write value 0x0 to scratchpad register */
-                Data = 0x00;
-                status = mos7840_set_uart_reg(serial->port[i],
-                                                SCRATCH_PAD_REGISTER, Data);
-                if (status < 0) {
-                        dev_dbg(&dev->dev, "Writing SCRATCH_PAD_REGISTER failed status-0x%x\n", status);
-                        break;
-                } else
-                        dev_dbg(&dev->dev, "SCRATCH_PAD_REGISTER Writing success status%d\n", status);
+        /* write values in clkstart0x0 and clkmulti 0x20 */
+        Data = 0x0;
+        status = mos7840_set_reg_sync(port, CLK_START_VALUE_REGISTER, Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing CLK_START_VALUE_REGISTER failed status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "CLK_START_VALUE_REGISTER Writing success status%d\n", status);
 
-                /* Zero Length flag register */
-                if ((mos7840_port->port_num != 1)
-                    && (serial->num_ports == 2)) {
+        Data = 0x20;
+        status = mos7840_set_reg_sync(port, CLK_MULTI_REGISTER, Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing CLK_MULTI_REGISTER failed status-0x%x\n", status);
+                goto error;
+        } else
+                dev_dbg(&port->dev, "CLK_MULTI_REGISTER Writing success status%d\n", status);
 
-                        Data = 0xff;
-                        status = mos7840_set_reg_sync(serial->port[i],
-                                      (__u16) (ZLP_REG1 +
-                                      ((__u16)mos7840_port->port_num)), Data);
-                        dev_dbg(&dev->dev, "ZLIP offset %x\n",
+        /* write value 0x0 to scratchpad register */
+        Data = 0x00;
+        status = mos7840_set_uart_reg(port, SCRATCH_PAD_REGISTER, Data);
+        if (status < 0) {
+                dev_dbg(&port->dev, "Writing SCRATCH_PAD_REGISTER failed status-0x%x\n", status);
+                goto out;
+        } else
+                dev_dbg(&port->dev, "SCRATCH_PAD_REGISTER Writing success status%d\n", status);
+
+        /* Zero Length flag register */
+        if ((mos7840_port->port_num != 1) && (serial->num_ports == 2)) {
+                Data = 0xff;
+                status = mos7840_set_reg_sync(port,
+                                (__u16) (ZLP_REG1 +
+                                        ((__u16)mos7840_port->port_num)), Data);
+                dev_dbg(&port->dev, "ZLIP offset %x\n",
                                 (__u16)(ZLP_REG1 + ((__u16) mos7840_port->port_num)));
-                        if (status < 0) {
-                                dev_dbg(&dev->dev, "Writing ZLP_REG%d failed status-0x%x\n", i + 2, status);
-                                break;
-                        } else
-                                dev_dbg(&dev->dev, "ZLP_REG%d Writing success status%d\n", i + 2, status);
-                } else {
-                        Data = 0xff;
-                        status = mos7840_set_reg_sync(serial->port[i],
-                              (__u16) (ZLP_REG1 +
-                              ((__u16)mos7840_port->port_num) - 0x1), Data);
-                        dev_dbg(&dev->dev, "ZLIP offset %x\n",
+                if (status < 0) {
+                        dev_dbg(&port->dev, "Writing ZLP_REG%d failed status-0x%x\n", pnum + 2, status);
+                        goto out;
+                } else
+                        dev_dbg(&port->dev, "ZLP_REG%d Writing success status%d\n", pnum + 2, status);
+        } else {
+                Data = 0xff;
+                status = mos7840_set_reg_sync(port,
+                                (__u16) (ZLP_REG1 +
+                                        ((__u16)mos7840_port->port_num) - 0x1), Data);
+                dev_dbg(&port->dev, "ZLIP offset %x\n",
                                 (__u16)(ZLP_REG1 + ((__u16) mos7840_port->port_num) - 0x1));
-                        if (status < 0) {
-                                dev_dbg(&dev->dev, "Writing ZLP_REG%d failed status-0x%x\n", i + 1, status);
-                                break;
-                        } else
-                                dev_dbg(&dev->dev, "ZLP_REG%d Writing success status%d\n", i + 1, status);
+                if (status < 0) {
+                        dev_dbg(&port->dev, "Writing ZLP_REG%d failed status-0x%x\n", pnum + 1, status);
+                        goto out;
+                } else
+                        dev_dbg(&port->dev, "ZLP_REG%d Writing success status%d\n", pnum + 1, status);
 
-                }
-                mos7840_port->control_urb = usb_alloc_urb(0, GFP_KERNEL);
-                mos7840_port->ctrl_buf = kmalloc(16, GFP_KERNEL);
-                mos7840_port->dr = kmalloc(sizeof(struct usb_ctrlrequest),
-                                                                GFP_KERNEL);
-                if (!mos7840_port->control_urb || !mos7840_port->ctrl_buf ||
-                                                        !mos7840_port->dr) {
-                        status = -ENOMEM;
-                        goto error;
-                }
+        }
+        mos7840_port->control_urb = usb_alloc_urb(0, GFP_KERNEL);
+        mos7840_port->ctrl_buf = kmalloc(16, GFP_KERNEL);
+        mos7840_port->dr = kmalloc(sizeof(struct usb_ctrlrequest),
+                        GFP_KERNEL);
+        if (!mos7840_port->control_urb || !mos7840_port->ctrl_buf ||
+                        !mos7840_port->dr) {
+                status = -ENOMEM;
+                goto error;
+        }
 
-                mos7840_port->has_led = false;
+        mos7840_port->has_led = false;
 
-                /* Initialize LED timers */
-                if (device_type == MOSCHIP_DEVICE_ID_7810) {
-                        mos7840_port->has_led = true;
+        /* Initialize LED timers */
+        if (device_type == MOSCHIP_DEVICE_ID_7810) {
+                mos7840_port->has_led = true;
 
-                        init_timer(&mos7840_port->led_timer1);
-                        mos7840_port->led_timer1.function = mos7840_led_off;
-                        mos7840_port->led_timer1.expires =
-                                        jiffies + msecs_to_jiffies(LED_ON_MS);
-                        mos7840_port->led_timer1.data =
-                                                (unsigned long)mos7840_port;
+                init_timer(&mos7840_port->led_timer1);
+                mos7840_port->led_timer1.function = mos7840_led_off;
+                mos7840_port->led_timer1.expires =
+                        jiffies + msecs_to_jiffies(LED_ON_MS);
+                mos7840_port->led_timer1.data = (unsigned long)mos7840_port;
 
-                        init_timer(&mos7840_port->led_timer2);
-                        mos7840_port->led_timer2.function =
-                                                mos7840_led_flag_off;
-                        mos7840_port->led_timer2.expires =
-                                        jiffies + msecs_to_jiffies(LED_OFF_MS);
-                        mos7840_port->led_timer2.data =
-                                                (unsigned long)mos7840_port;
+                init_timer(&mos7840_port->led_timer2);
+                mos7840_port->led_timer2.function = mos7840_led_flag_off;
+                mos7840_port->led_timer2.expires =
+                        jiffies + msecs_to_jiffies(LED_OFF_MS);
+                mos7840_port->led_timer2.data = (unsigned long)mos7840_port;
 
-                        mos7840_port->led_flag = false;
+                mos7840_port->led_flag = false;
 
-                        /* Turn off LED */
-                        mos7840_set_led_sync(serial->port[i],
-                                                MODEM_CONTROL_REGISTER, 0x0300);
-                }
+                /* Turn off LED */
+                mos7840_set_led_sync(port, MODEM_CONTROL_REGISTER, 0x0300);
         }
+out:
+        if (pnum == serial->num_ports - 1) {
+                /* Zero Length flag enable */
+                Data = 0x0f;
+                status = mos7840_set_reg_sync(serial->port[0], ZLP_REG5, Data);
+                if (status < 0) {
+                        dev_dbg(&port->dev, "Writing ZLP_REG5 failed status-0x%x\n", status);
+                        goto error;
+                } else
+                        dev_dbg(&port->dev, "ZLP_REG5 Writing success status%d\n", status);
 
-        /* Zero Length flag enable */
-        Data = 0x0f;
-        status = mos7840_set_reg_sync(serial->port[0], ZLP_REG5, Data);
-        if (status < 0) {
-                dev_dbg(&dev->dev, "Writing ZLP_REG5 failed status-0x%x\n", status);
-                goto error;
-        } else
-                dev_dbg(&dev->dev, "ZLP_REG5 Writing success status%d\n", status);
-
-        /* setting configuration feature to one */
-        usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-                        (__u8) 0x03, 0x00, 0x01, 0x00, NULL, 0x00, MOS_WDR_TIMEOUT);
+                /* setting configuration feature to one */
+                usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
+                                0x03, 0x00, 0x01, 0x00, NULL, 0x00,
+                                MOS_WDR_TIMEOUT);
+        }
         return 0;
 error:
-        for (/* nothing */; i >= 0; i--) {
-                mos7840_port = mos7840_get_port_private(serial->port[i]);
+        kfree(mos7840_port->dr);
+        kfree(mos7840_port->ctrl_buf);
+        usb_free_urb(mos7840_port->control_urb);
+        kfree(mos7840_port);
 
-                kfree(mos7840_port->dr);
-                kfree(mos7840_port->ctrl_buf);
-                usb_free_urb(mos7840_port->control_urb);
-                kfree(mos7840_port);
-                serial->port[i] = NULL;
-        }
         return status;
 }
 
-/****************************************************************************
- * mos7840_disconnect
- *      This function is called whenever the device is removed from the usb bus.
- ****************************************************************************/
-
-static void mos7840_disconnect(struct usb_serial *serial)
+static int mos7840_port_remove(struct usb_serial_port *port)
 {
-        int i;
-        unsigned long flags;
         struct moschip_port *mos7840_port;
 
-        /* check for the ports to be closed,close the ports and disconnect */
+        mos7840_port = mos7840_get_port_private(port);
 
-        /* free private structure allocated for serial port  *
-         * stop reads and writes on all ports                */
+        if (mos7840_port->has_led) {
+                /* Turn off LED */
+                mos7840_set_led_sync(port, MODEM_CONTROL_REGISTER, 0x0300);
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                mos7840_port = mos7840_get_port_private(serial->port[i]);
-                if (mos7840_port) {
-                        spin_lock_irqsave(&mos7840_port->pool_lock, flags);
-                        mos7840_port->zombie = 1;
-                        spin_unlock_irqrestore(&mos7840_port->pool_lock, flags);
-                        usb_kill_urb(mos7840_port->control_urb);
-                }
+                del_timer_sync(&mos7840_port->led_timer1);
+                del_timer_sync(&mos7840_port->led_timer2);
         }
-}
-
-/****************************************************************************
- * mos7840_release
- *      This function is called when the usb_serial structure is freed.
- ****************************************************************************/
-
-static void mos7840_release(struct usb_serial *serial)
-{
-        int i;
-        struct moschip_port *mos7840_port;
-
-        /* check for the ports to be closed,close the ports and disconnect */
+        usb_kill_urb(mos7840_port->control_urb);
+        usb_free_urb(mos7840_port->control_urb);
+        kfree(mos7840_port->ctrl_buf);
+        kfree(mos7840_port->dr);
+        kfree(mos7840_port);
 
-        /* free private structure allocated for serial port  *
-         * stop reads and writes on all ports                */
-
-        for (i = 0; i < serial->num_ports; ++i) {
-                mos7840_port = mos7840_get_port_private(serial->port[i]);
-                if (mos7840_port) {
-                        if (mos7840_port->has_led) {
-                                /* Turn off LED */
-                                mos7840_set_led_sync(mos7840_port->port,
-                                                MODEM_CONTROL_REGISTER, 0x0300);
-
-                                del_timer_sync(&mos7840_port->led_timer1);
-                                del_timer_sync(&mos7840_port->led_timer2);
-                        }
-                        kfree(mos7840_port->ctrl_buf);
-                        kfree(mos7840_port->dr);
-                        kfree(mos7840_port);
-                }
-        }
+        return 0;
 }
 
 static struct usb_serial_driver moschip7840_4port_device = {
@@ -2677,9 +2597,8 @@ static struct usb_serial_driver moschip7840_4port_device = {
         .tiocmget = mos7840_tiocmget,
         .tiocmset = mos7840_tiocmset,
         .get_icount = mos7840_get_icount,
-        .attach = mos7840_startup,
-        .disconnect = mos7840_disconnect,
-        .release = mos7840_release,
+        .port_probe = mos7840_port_probe,
+        .port_remove = mos7840_port_remove,
         .read_bulk_callback = mos7840_bulk_in_callback,
         .read_int_callback = mos7840_interrupt_callback,
 };
diff --git a/drivers/usb/serial/omninet.c b/drivers/usb/serial/omninet.c
index 6def58b79382..9ab73d295774 100644
--- a/drivers/usb/serial/omninet.c
+++ b/drivers/usb/serial/omninet.c
@@ -44,8 +44,8 @@ static int  omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
                                 const unsigned char *buf, int count);
 static int  omninet_write_room(struct tty_struct *tty);
 static void omninet_disconnect(struct usb_serial *serial);
-static void omninet_release(struct usb_serial *serial);
-static int omninet_attach(struct usb_serial *serial);
+static int omninet_port_probe(struct usb_serial_port *port);
+static int omninet_port_remove(struct usb_serial_port *port);
 
 static const struct usb_device_id id_table[] = {
         { USB_DEVICE(ZYXEL_VENDOR_ID, ZYXEL_OMNINET_ID) },
@@ -62,7 +62,8 @@ static struct usb_serial_driver zyxel_omninet_device = {
         .description =          "ZyXEL - omni.net lcd plus usb",
         .id_table =             id_table,
         .num_ports =            1,
-        .attach =               omninet_attach,
+        .port_probe =           omninet_port_probe,
+        .port_remove =          omninet_port_remove,
         .open =                 omninet_open,
         .close =                omninet_close,
         .write =                omninet_write,
@@ -70,7 +71,6 @@ static struct usb_serial_driver zyxel_omninet_device = {
         .read_bulk_callback =   omninet_read_bulk_callback,
         .write_bulk_callback =  omninet_write_bulk_callback,
         .disconnect =           omninet_disconnect,
-        .release =              omninet_release,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -112,18 +112,26 @@ struct omninet_data {
         __u8    od_outseq;      /* Sequence number for bulk_out URBs */
 };
 
-static int omninet_attach(struct usb_serial *serial)
+static int omninet_port_probe(struct usb_serial_port *port)
 {
         struct omninet_data *od;
-        struct usb_serial_port *port = serial->port[0];
 
         od = kmalloc(sizeof(struct omninet_data), GFP_KERNEL);
-        if (!od) {
-                dev_err(&port->dev, "%s- kmalloc(%Zd) failed.\n",
-                        __func__, sizeof(struct omninet_data));
+        if (!od)
                 return -ENOMEM;
-        }
+
         usb_set_serial_port_data(port, od);
+
+        return 0;
+}
+
+static int omninet_port_remove(struct usb_serial_port *port)
+{
+        struct omninet_data *od;
+
+        od = usb_get_serial_port_data(port);
+        kfree(od);
+
         return 0;
 }
 
@@ -279,14 +287,6 @@ static void omninet_disconnect(struct usb_serial *serial)
         usb_kill_urb(wport->write_urb);
 }
 
-
-static void omninet_release(struct usb_serial *serial)
-{
-        struct usb_serial_port *port = serial->port[0];
-
-        kfree(usb_get_serial_port_data(port));
-}
-
 module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/opticon.c b/drivers/usb/serial/opticon.c
index 41b1647306eb..6aba731d4864 100644
--- a/drivers/usb/serial/opticon.c
+++ b/drivers/usb/serial/opticon.c
@@ -155,7 +155,11 @@ static int send_control_msg(struct usb_serial_port *port, u8 requesttype,
 {
         struct usb_serial *serial = port->serial;
         int retval;
-        u8 buffer[2];
+        u8 *buffer;
+
+        buffer = kzalloc(1, GFP_KERNEL);
+        if (!buffer)
+                return -ENOMEM;
 
         buffer[0] = val;
         /* Send the message to the vendor control endpoint
@@ -164,6 +168,7 @@ static int send_control_msg(struct usb_serial_port *port, u8 requesttype,
                                 requesttype,
                                 USB_DIR_OUT|USB_TYPE_VENDOR|USB_RECIP_INTERFACE,
                                 0, 0, buffer, 1, 0);
+        kfree(buffer);
 
         return retval;
 }
@@ -281,7 +286,7 @@ static int opticon_write(struct tty_struct *tty, struct usb_serial_port *port,
         if (!dr) {
                 dev_err(&port->dev, "out of memory\n");
                 count = -ENOMEM;
-                goto error;
+                goto error_no_dr;
         }
 
         dr->bRequestType = USB_TYPE_VENDOR | USB_RECIP_INTERFACE | USB_DIR_OUT;
@@ -311,6 +316,8 @@ static int opticon_write(struct tty_struct *tty, struct usb_serial_port *port,
 
         return count;
 error:
+        kfree(dr);
+error_no_dr:
         usb_free_urb(urb);
 error_no_urb:
         kfree(buffer);
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index 30cff03e9f01..5dee7d61241e 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -47,6 +47,7 @@
 /* Function prototypes */
 static int  option_probe(struct usb_serial *serial,
                         const struct usb_device_id *id);
+static int option_attach(struct usb_serial *serial);
 static void option_release(struct usb_serial *serial);
 static int option_send_setup(struct usb_serial_port *port);
 static void option_instat_callback(struct urb *urb);
@@ -503,11 +504,19 @@ static const struct option_blacklist_info net_intf5_blacklist = {
         .reserved = BIT(5),
 };
 
+static const struct option_blacklist_info net_intf6_blacklist = {
+        .reserved = BIT(6),
+};
+
 static const struct option_blacklist_info zte_mf626_blacklist = {
         .sendsetup = BIT(0) | BIT(1),
         .reserved = BIT(4),
 };
 
+static const struct option_blacklist_info zte_1255_blacklist = {
+        .reserved = BIT(3) | BIT(4),
+};
+
 static const struct usb_device_id option_ids[] = {
         { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_COLT) },
         { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA) },
@@ -853,13 +862,19 @@ static const struct usb_device_id option_ids[] = {
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0113, 0xff, 0xff, 0xff),
                 .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0117, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0118, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0121, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0118, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0121, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0122, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0123, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0124, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0125, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0126, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0123, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0124, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0125, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf6_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0126, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0128, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0142, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0143, 0xff, 0xff, 0xff) },
@@ -872,7 +887,8 @@ static const struct usb_device_id option_ids[] = {
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0156, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0157, 0xff, 0xff, 0xff),
           .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0158, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0158, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0159, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0161, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0162, 0xff, 0xff, 0xff) },
@@ -880,13 +896,22 @@ static const struct usb_device_id option_ids[] = {
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0165, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0167, 0xff, 0xff, 0xff),
           .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0191, 0xff, 0xff, 0xff), /* ZTE EuFi890 */
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0199, 0xff, 0xff, 0xff), /* ZTE MF820S */
+          .driver_info = (kernel_ulong_t)&net_intf1_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0257, 0xff, 0xff, 0xff), /* ZTE MF821 */
+          .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0326, 0xff, 0xff, 0xff),
           .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1008, 0xff, 0xff, 0xff),
           .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1010, 0xff, 0xff, 0xff),
           .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1012, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1012, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1021, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1057, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1058, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1059, 0xff, 0xff, 0xff) },
@@ -1002,18 +1027,24 @@ static const struct usb_device_id option_ids[] = {
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1169, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1170, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1244, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1245, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1245, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1246, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1247, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1247, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1248, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1249, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1250, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1251, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1252, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1252, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1253, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1254, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1255, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1256, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1254, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1255, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&zte_1255_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1256, 0xff, 0xff, 0xff),
+          .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1257, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1258, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1259, 0xff, 0xff, 0xff) },
@@ -1058,8 +1089,16 @@ static const struct usb_device_id option_ids[] = {
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1298, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1299, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1300, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1401, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1402, 0xff, 0xff, 0xff),
                 .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1424, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1425, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1426, 0xff, 0xff, 0xff),  /* ZTE MF91 */
+                .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x2002, 0xff,
           0xff, 0xff), .driver_info = (kernel_ulong_t)&zte_k3765_z_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x2003, 0xff, 0xff, 0xff) },
@@ -1071,15 +1110,21 @@ static const struct usb_device_id option_ids[] = {
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0070, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0073, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0094, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0130, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0133, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0141, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0130, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf1_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0133, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0141, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0147, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0152, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0168, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0168, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0170, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0176, 0xff, 0xff, 0xff) },
-        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0178, 0xff, 0xff, 0xff) },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0176, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
+        { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0178, 0xff, 0xff, 0xff),
+                .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
 
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_CDMA_TECH, 0xff, 0xff, 0xff) },
         { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC8710, 0xff, 0xff, 0xff) },
@@ -1244,8 +1289,9 @@ static struct usb_serial_driver option_1port_device = {
         .tiocmget          = usb_wwan_tiocmget,
         .tiocmset          = usb_wwan_tiocmset,
         .ioctl             = usb_wwan_ioctl,
-        .attach            = usb_wwan_startup,
+        .attach            = option_attach,
         .release           = option_release,
+        .port_probe        = usb_wwan_port_probe,
         .port_remove       = usb_wwan_port_remove,
         .read_int_callback = option_instat_callback,
 #ifdef CONFIG_PM
@@ -1291,8 +1337,6 @@ static bool is_blacklisted(const u8 ifnum, enum option_blacklist_reason reason,
 static int option_probe(struct usb_serial *serial,
                         const struct usb_device_id *id)
 {
-        struct usb_wwan_intf_private *data;
-        struct option_private *priv;
         struct usb_interface_descriptor *iface_desc =
                                 &serial->interface->cur_altsetting->desc;
         struct usb_device_descriptor *dev_desc = &serial->dev->descriptor;
@@ -1330,6 +1374,19 @@ static int option_probe(struct usb_serial *serial,
                 iface_desc->bInterfaceClass != USB_CLASS_CDC_DATA)
                 return -ENODEV;
 
+        /* Store device id so we can use it during attach. */
+        usb_set_serial_data(serial, (void *)id);
+
+        return 0;
+}
+
+static int option_attach(struct usb_serial *serial)
+{
+        struct usb_interface_descriptor *iface_desc;
+        const struct usb_device_id *id;
+        struct usb_wwan_intf_private *data;
+        struct option_private *priv;
+
         data = kzalloc(sizeof(struct usb_wwan_intf_private), GFP_KERNEL);
         if (!data)
                 return -ENOMEM;
@@ -1340,6 +1397,10 @@ static int option_probe(struct usb_serial *serial,
                 return -ENOMEM;
         }
 
+        /* Retrieve device id stored at probe. */
+        id = usb_get_serial_data(serial);
+        iface_desc = &serial->interface->cur_altsetting->desc;
+
         priv->bInterfaceNumber = iface_desc->bInterfaceNumber;
         data->private = priv;
 
diff --git a/drivers/usb/serial/oti6858.c b/drivers/usb/serial/oti6858.c
index 933241f03fd8..cee9a52ca891 100644
--- a/drivers/usb/serial/oti6858.c
+++ b/drivers/usb/serial/oti6858.c
@@ -137,8 +137,8 @@ static int oti6858_chars_in_buffer(struct tty_struct *tty);
 static int oti6858_tiocmget(struct tty_struct *tty);
 static int oti6858_tiocmset(struct tty_struct *tty,
                                 unsigned int set, unsigned int clear);
-static int oti6858_startup(struct usb_serial *serial);
-static void oti6858_release(struct usb_serial *serial);
+static int oti6858_port_probe(struct usb_serial_port *port);
+static int oti6858_port_remove(struct usb_serial_port *port);
 
 /* device info */
 static struct usb_serial_driver oti6858_device = {
@@ -161,8 +161,8 @@ static struct usb_serial_driver oti6858_device = {
         .write_bulk_callback =  oti6858_write_bulk_callback,
         .write_room =           oti6858_write_room,
         .chars_in_buffer =      oti6858_chars_in_buffer,
-        .attach =               oti6858_startup,
-        .release =              oti6858_release,
+        .port_probe =           oti6858_port_probe,
+        .port_remove =          oti6858_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -331,36 +331,33 @@ static void send_data(struct work_struct *work)
         usb_serial_port_softint(port);
 }
 
-static int oti6858_startup(struct usb_serial *serial)
+static int oti6858_port_probe(struct usb_serial_port *port)
 {
-        struct usb_serial_port *port = serial->port[0];
         struct oti6858_private *priv;
-        int i;
-
-        for (i = 0; i < serial->num_ports; ++i) {
-                priv = kzalloc(sizeof(struct oti6858_private), GFP_KERNEL);
-                if (!priv)
-                        break;
-
-                spin_lock_init(&priv->lock);
-                init_waitqueue_head(&priv->intr_wait);
-/*              INIT_WORK(&priv->setup_work, setup_line, serial->port[i]); */
-/*              INIT_WORK(&priv->write_work, send_data, serial->port[i]); */
-                priv->port = port;
-                INIT_DELAYED_WORK(&priv->delayed_setup_work, setup_line);
-                INIT_DELAYED_WORK(&priv->delayed_write_work, send_data);
-
-                usb_set_serial_port_data(serial->port[i], priv);
-        }
-        if (i == serial->num_ports)
-                return 0;
 
-        for (--i; i >= 0; --i) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
-        return -ENOMEM;
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        spin_lock_init(&priv->lock);
+        init_waitqueue_head(&priv->intr_wait);
+        priv->port = port;
+        INIT_DELAYED_WORK(&priv->delayed_setup_work, setup_line);
+        INIT_DELAYED_WORK(&priv->delayed_write_work, send_data);
+
+        usb_set_serial_port_data(port, priv);
+
+        return 0;
+}
+
+static int oti6858_port_remove(struct usb_serial_port *port)
+{
+        struct oti6858_private *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 static int oti6858_write(struct tty_struct *tty, struct usb_serial_port *port,
@@ -709,15 +706,6 @@ static int oti6858_ioctl(struct tty_struct *tty,
         return -ENOIOCTLCMD;
 }
 
-
-static void oti6858_release(struct usb_serial *serial)
-{
-        int i;
-
-        for (i = 0; i < serial->num_ports; ++i)
-                kfree(usb_get_serial_port_data(serial->port[i]));
-}
-
 static void oti6858_read_int_callback(struct urb *urb)
 {
         struct usb_serial_port *port =  urb->context;
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index 892ebdc7a364..600241901361 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -133,12 +133,15 @@ enum pl2303_type {
         HX,             /* HX version of the pl2303 chip */
 };
 
+struct pl2303_serial_private {
+        enum pl2303_type type;
+};
+
 struct pl2303_private {
         spinlock_t lock;
         wait_queue_head_t delta_msr_wait;
         u8 line_control;
         u8 line_status;
-        enum pl2303_type type;
 };
 
 static int pl2303_vendor_read(__u16 value, __u16 index,
@@ -167,14 +170,19 @@ static int pl2303_vendor_write(__u16 value, __u16 index,
 
 static int pl2303_startup(struct usb_serial *serial)
 {
-        struct pl2303_private *priv;
+        struct pl2303_serial_private *spriv;
         enum pl2303_type type = type_0;
         unsigned char *buf;
-        int i;
+
+        spriv = kzalloc(sizeof(*spriv), GFP_KERNEL);
+        if (!spriv)
+                return -ENOMEM;
 
         buf = kmalloc(10, GFP_KERNEL);
-        if (buf == NULL)
+        if (!buf) {
+                kfree(spriv);
                 return -ENOMEM;
+        }
 
         if (serial->dev->descriptor.bDeviceClass == 0x02)
                 type = type_0;
@@ -186,15 +194,8 @@ static int pl2303_startup(struct usb_serial *serial)
                 type = type_1;
         dev_dbg(&serial->interface->dev, "device type: %d\n", type);
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                priv = kzalloc(sizeof(struct pl2303_private), GFP_KERNEL);
-                if (!priv)
-                        goto cleanup;
-                spin_lock_init(&priv->lock);
-                init_waitqueue_head(&priv->delta_msr_wait);
-                priv->type = type;
-                usb_set_serial_port_data(serial->port[i], priv);
-        }
+        spriv->type = type;
+        usb_set_serial_data(serial, spriv);
 
         pl2303_vendor_read(0x8484, 0, serial, buf);
         pl2303_vendor_write(0x0404, 0, serial);
@@ -213,15 +214,40 @@ static int pl2303_startup(struct usb_serial *serial)
 
         kfree(buf);
         return 0;
+}
 
-cleanup:
-        kfree(buf);
-        for (--i; i >= 0; --i) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
-        return -ENOMEM;
+static void pl2303_release(struct usb_serial *serial)
+{
+        struct pl2303_serial_private *spriv;
+
+        spriv = usb_get_serial_data(serial);
+        kfree(spriv);
+}
+
+static int pl2303_port_probe(struct usb_serial_port *port)
+{
+        struct pl2303_private *priv;
+
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        spin_lock_init(&priv->lock);
+        init_waitqueue_head(&priv->delta_msr_wait);
+
+        usb_set_serial_port_data(port, priv);
+
+        return 0;
+}
+
+static int pl2303_port_remove(struct usb_serial_port *port)
+{
+        struct pl2303_private *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 static int set_control_lines(struct usb_device *dev, u8 value)
@@ -240,6 +266,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
                 struct usb_serial_port *port, struct ktermios *old_termios)
 {
         struct usb_serial *serial = port->serial;
+        struct pl2303_serial_private *spriv = usb_get_serial_data(serial);
         struct pl2303_private *priv = usb_get_serial_port_data(port);
         unsigned long flags;
         unsigned int cflag;
@@ -323,7 +350,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
                 }
                 if (baud > 1228800) {
                         /* type_0, type_1 only support up to 1228800 baud */
-                        if (priv->type != HX)
+                        if (spriv->type != HX)
                                 baud = 1228800;
                         else if (baud > 6000000)
                                 baud = 6000000;
@@ -426,7 +453,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
              buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6]);
 
         if (cflag & CRTSCTS) {
-                if (priv->type == HX)
+                if (spriv->type == HX)
                         pl2303_vendor_write(0x0, 0x61, serial);
                 else
                         pl2303_vendor_write(0x0, 0x41, serial);
@@ -468,10 +495,10 @@ static int pl2303_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
         struct ktermios tmp_termios;
         struct usb_serial *serial = port->serial;
-        struct pl2303_private *priv = usb_get_serial_port_data(port);
+        struct pl2303_serial_private *spriv = usb_get_serial_data(serial);
         int result;
 
-        if (priv->type != HX) {
+        if (spriv->type != HX) {
                 usb_clear_halt(serial->dev, port->write_urb->pipe);
                 usb_clear_halt(serial->dev, port->read_urb->pipe);
         } else {
@@ -655,17 +682,6 @@ static void pl2303_break_ctl(struct tty_struct *tty, int break_state)
                 dev_err(&port->dev, "error sending break = %d\n", result);
 }
 
-static void pl2303_release(struct usb_serial *serial)
-{
-        int i;
-        struct pl2303_private *priv;
-
-        for (i = 0; i < serial->num_ports; ++i) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-        }
-}
-
 static void pl2303_update_line_status(struct usb_serial_port *port,
                                       unsigned char *data,
                                       unsigned int actual_length)
@@ -827,6 +843,8 @@ static struct usb_serial_driver pl2303_device = {
         .read_int_callback =    pl2303_read_int_callback,
         .attach =               pl2303_startup,
         .release =              pl2303_release,
+        .port_probe =           pl2303_port_probe,
+        .port_remove =          pl2303_port_remove,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index c3ddb65c05f2..aa148c21ea40 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -138,7 +138,6 @@ MODULE_DEVICE_TABLE(usb, id_table);
 
 static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 {
-        struct usb_wwan_intf_private *data;
         struct usb_host_interface *intf = serial->interface->cur_altsetting;
         struct device *dev = &serial->dev->dev;
         int retval = -ENODEV;
@@ -154,13 +153,6 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
         ifnum = intf->desc.bInterfaceNumber;
         dev_dbg(dev, "This Interface = %d\n", ifnum);
 
-        data = kzalloc(sizeof(struct usb_wwan_intf_private),
-                                         GFP_KERNEL);
-        if (!data)
-                return -ENOMEM;
-
-        spin_lock_init(&data->susp_lock);
-
         if (nintf == 1) {
                 /* QDL mode */
                 /* Gobi 2000 has a single altsetting, older ones have two */
@@ -253,20 +245,28 @@ done:
                 }
         }
 
-        /* Set serial->private if not returning error */
-        if (retval == 0)
-                usb_set_serial_data(serial, data);
-        else
-                kfree(data);
-
         return retval;
 }
 
+static int qc_attach(struct usb_serial *serial)
+{
+        struct usb_wwan_intf_private *data;
+
+        data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (!data)
+                return -ENOMEM;
+
+        spin_lock_init(&data->susp_lock);
+
+        usb_set_serial_data(serial, data);
+
+        return 0;
+}
+
 static void qc_release(struct usb_serial *serial)
 {
         struct usb_wwan_intf_private *priv = usb_get_serial_data(serial);
 
-        /* Free the private data allocated in qcprobe */
         usb_set_serial_data(serial, NULL);
         kfree(priv);
 }
@@ -285,8 +285,9 @@ static struct usb_serial_driver qcdevice = {
         .write               = usb_wwan_write,
         .write_room          = usb_wwan_write_room,
         .chars_in_buffer     = usb_wwan_chars_in_buffer,
-        .attach              = usb_wwan_startup,
+        .attach              = qc_attach,
         .release             = qc_release,
+        .port_probe          = usb_wwan_port_probe,
         .port_remove         = usb_wwan_port_remove,
 #ifdef CONFIG_PM
         .suspend             = usb_wwan_suspend,
diff --git a/drivers/usb/serial/quatech2.c b/drivers/usb/serial/quatech2.c
index 2cdfdcc90b37..ffcfc962ab10 100644
--- a/drivers/usb/serial/quatech2.c
+++ b/drivers/usb/serial/quatech2.c
@@ -143,12 +143,12 @@ static void qt2_read_bulk_callback(struct urb *urb);
 
 static void qt2_release(struct usb_serial *serial)
 {
-        int i;
+        struct qt2_serial_private *serial_priv;
 
-        kfree(usb_get_serial_data(serial));
+        serial_priv = usb_get_serial_data(serial);
 
-        for (i = 0; i < serial->num_ports; i++)
-                kfree(usb_get_serial_port_data(serial->port[i]));
+        usb_free_urb(serial_priv->read_urb);
+        kfree(serial_priv);
 }
 
 static inline int calc_baud_divisor(int baudrate)
@@ -423,11 +423,16 @@ static void qt2_close(struct usb_serial_port *port)
         port_priv->is_open = false;
 
         spin_lock_irqsave(&port_priv->urb_lock, flags);
-        if (port_priv->write_urb->status == -EINPROGRESS)
-                usb_kill_urb(port_priv->write_urb);
+        usb_kill_urb(port_priv->write_urb);
         port_priv->urb_in_use = false;
         spin_unlock_irqrestore(&port_priv->urb_lock, flags);
 
+        mutex_lock(&port->serial->disc_mutex);
+        if (port->serial->disconnected) {
+                mutex_unlock(&port->serial->disc_mutex);
+                return;
+        }
+
         /* flush the port transmit buffer */
         i = usb_control_msg(serial->dev,
                             usb_rcvctrlpipe(serial->dev, 0),
@@ -459,26 +464,14 @@ static void qt2_close(struct usb_serial_port *port)
                 dev_err(&port->dev, "%s - close port failed %i\n",
                         __func__, i);
 
+        mutex_unlock(&port->serial->disc_mutex);
 }
 
 static void qt2_disconnect(struct usb_serial *serial)
 {
         struct qt2_serial_private *serial_priv = usb_get_serial_data(serial);
-        struct qt2_port_private *port_priv;
-        int i;
-
-        if (serial_priv->read_urb->status == -EINPROGRESS)
-                usb_kill_urb(serial_priv->read_urb);
-
-        usb_free_urb(serial_priv->read_urb);
 
-        for (i = 0; i < serial->num_ports; i++) {
-                port_priv = usb_get_serial_port_data(serial->port[i]);
-
-                if (port_priv->write_urb->status == -EINPROGRESS)
-                        usb_kill_urb(port_priv->write_urb);
-                usb_free_urb(port_priv->write_urb);
-        }
+        usb_kill_urb(serial_priv->read_urb);
 }
 
 static int get_serial_info(struct usb_serial_port *port,
@@ -773,11 +766,9 @@ static void qt2_read_bulk_callback(struct urb *urb)
 
 static int qt2_setup_urbs(struct usb_serial *serial)
 {
-        struct usb_serial_port *port;
         struct usb_serial_port *port0;
         struct qt2_serial_private *serial_priv;
-        struct qt2_port_private *port_priv;
-        int pcount, status;
+        int status;
 
         port0 = serial->port[0];
 
@@ -795,46 +786,21 @@ static int qt2_setup_urbs(struct usb_serial *serial)
                           sizeof(serial_priv->read_buffer),
                           qt2_read_bulk_callback, serial);
 
-        /* setup write_urb for each port */
-        for (pcount = 0; pcount < serial->num_ports; pcount++) {
-
-                port = serial->port[pcount];
-                port_priv = usb_get_serial_port_data(port);
-
-                port_priv->write_urb = usb_alloc_urb(0, GFP_KERNEL);
-                if (!port_priv->write_urb) {
-                        dev_err(&serial->dev->dev,
-                                "failed to alloc write_urb for port %i\n",
-                                pcount);
-                        return -ENOMEM;
-                }
-
-                usb_fill_bulk_urb(port_priv->write_urb,
-                                  serial->dev,
-                                  usb_sndbulkpipe(serial->dev,
-                                                  port0->
-                                                  bulk_out_endpointAddress),
-                                  port_priv->write_buffer,
-                                  sizeof(port_priv->write_buffer),
-                                  qt2_write_bulk_callback, port);
-        }
-
         status = usb_submit_urb(serial_priv->read_urb, GFP_KERNEL);
         if (status != 0) {
                 dev_err(&serial->dev->dev,
                         "%s - submit read urb failed %i\n", __func__, status);
+                usb_free_urb(serial_priv->read_urb);
                 return status;
         }
 
         return 0;
-
 }
 
 static int qt2_attach(struct usb_serial *serial)
 {
         struct qt2_serial_private *serial_priv;
-        struct qt2_port_private *port_priv;
-        int status, pcount;
+        int status;
 
         /* power on unit */
         status = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
@@ -854,26 +820,6 @@ static int qt2_attach(struct usb_serial *serial)
 
         usb_set_serial_data(serial, serial_priv);
 
-        for (pcount = 0; pcount < serial->num_ports; pcount++) {
-                port_priv = kzalloc(sizeof(*port_priv), GFP_KERNEL);
-                if (!port_priv) {
-                        dev_err(&serial->dev->dev,
-                                "%s- kmalloc(%Zd) failed.\n", __func__,
-                                sizeof(*port_priv));
-                        pcount--;
-                        status = -ENOMEM;
-                        goto attach_failed;
-                }
-
-                spin_lock_init(&port_priv->lock);
-                spin_lock_init(&port_priv->urb_lock);
-                init_waitqueue_head(&port_priv->delta_msr_wait);
-
-                port_priv->port = serial->port[pcount];
-
-                usb_set_serial_port_data(serial->port[pcount], port_priv);
-        }
-
         status = qt2_setup_urbs(serial);
         if (status != 0)
                 goto attach_failed;
@@ -881,14 +827,53 @@ static int qt2_attach(struct usb_serial *serial)
         return 0;
 
 attach_failed:
-        for (/* empty */; pcount >= 0; pcount--) {
-                port_priv = usb_get_serial_port_data(serial->port[pcount]);
-                kfree(port_priv);
-        }
         kfree(serial_priv);
         return status;
 }
 
+static int qt2_port_probe(struct usb_serial_port *port)
+{
+        struct usb_serial *serial = port->serial;
+        struct qt2_port_private *port_priv;
+        u8 bEndpointAddress;
+
+        port_priv = kzalloc(sizeof(*port_priv), GFP_KERNEL);
+        if (!port_priv)
+                return -ENOMEM;
+
+        spin_lock_init(&port_priv->lock);
+        spin_lock_init(&port_priv->urb_lock);
+        init_waitqueue_head(&port_priv->delta_msr_wait);
+        port_priv->port = port;
+
+        port_priv->write_urb = usb_alloc_urb(0, GFP_KERNEL);
+        if (!port_priv->write_urb) {
+                kfree(port_priv);
+                return -ENOMEM;
+        }
+        bEndpointAddress = serial->port[0]->bulk_out_endpointAddress;
+        usb_fill_bulk_urb(port_priv->write_urb, serial->dev,
+                                usb_sndbulkpipe(serial->dev, bEndpointAddress),
+                                port_priv->write_buffer,
+                                sizeof(port_priv->write_buffer),
+                                qt2_write_bulk_callback, port);
+
+        usb_set_serial_port_data(port, port_priv);
+
+        return 0;
+}
+
+static int qt2_port_remove(struct usb_serial_port *port)
+{
+        struct qt2_port_private *port_priv;
+
+        port_priv = usb_get_serial_port_data(port);
+        usb_free_urb(port_priv->write_urb);
+        kfree(port_priv);
+
+        return 0;
+}
+
 static int qt2_tiocmget(struct tty_struct *tty)
 {
         struct usb_serial_port *port = tty->driver_data;
@@ -1127,6 +1112,8 @@ static struct usb_serial_driver qt2_device = {
         .attach              = qt2_attach,
         .release             = qt2_release,
         .disconnect          = qt2_disconnect,
+        .port_probe          = qt2_port_probe,
+        .port_remove         = qt2_port_remove,
         .dtr_rts             = qt2_dtr_rts,
         .break_ctl           = qt2_break_ctl,
         .tiocmget            = qt2_tiocmget,
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index 01d882cf3775..270860f6bb2a 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -161,7 +161,6 @@ static int sierra_probe(struct usb_serial *serial,
 {
         int result = 0;
         struct usb_device *udev;
-        struct sierra_intf_private *data;
         u8 ifnum;
 
         udev = serial->dev;
@@ -188,11 +187,6 @@ static int sierra_probe(struct usb_serial *serial,
                 return -ENODEV;
         }
 
-        data = serial->private = kzalloc(sizeof(struct sierra_intf_private), GFP_KERNEL);
-        if (!data)
-                return -ENOMEM;
-        spin_lock_init(&data->susp_lock);
-
         return result;
 }
 
@@ -884,11 +878,15 @@ static void sierra_dtr_rts(struct usb_serial_port *port, int on)
 
 static int sierra_startup(struct usb_serial *serial)
 {
-        struct usb_serial_port *port;
-        struct sierra_port_private *portdata;
-        struct sierra_iface_info *himemoryp = NULL;
-        int i;
-        u8 ifnum;
+        struct sierra_intf_private *intfdata;
+
+        intfdata = kzalloc(sizeof(*intfdata), GFP_KERNEL);
+        if (!intfdata)
+                return -ENOMEM;
+
+        spin_lock_init(&intfdata->susp_lock);
+
+        usb_set_serial_data(serial, intfdata);
 
         /* Set Device mode to D0 */
         sierra_set_power_state(serial->dev, 0x0000);
@@ -897,68 +895,71 @@ static int sierra_startup(struct usb_serial *serial)
         if (nmea)
                 sierra_vsc_set_nmea(serial->dev, 1);
 
-        /* Now setup per port private data */
-        for (i = 0; i < serial->num_ports; i++) {
-                port = serial->port[i];
-                portdata = kzalloc(sizeof(*portdata), GFP_KERNEL);
-                if (!portdata) {
-                        dev_dbg(&port->dev, "%s: kmalloc for "
-                                "sierra_port_private (%d) failed!\n",
-                                __func__, i);
-                        return -ENOMEM;
-                }
-                spin_lock_init(&portdata->lock);
-                init_usb_anchor(&portdata->active);
-                init_usb_anchor(&portdata->delayed);
-                ifnum = i;
-                /* Assume low memory requirements */
-                portdata->num_out_urbs = N_OUT_URB;
-                portdata->num_in_urbs  = N_IN_URB;
-
-                /* Determine actual memory requirements */
-                if (serial->num_ports == 1) {
-                        /* Get interface number for composite device */
-                        ifnum = sierra_calc_interface(serial);
-                        himemoryp =
-                            (struct sierra_iface_info *)&typeB_interface_list;
-                        if (is_himemory(ifnum, himemoryp)) {
-                                portdata->num_out_urbs = N_OUT_URB_HM;
-                                portdata->num_in_urbs  = N_IN_URB_HM;
-                        }
-                }
-                else {
-                        himemoryp =
-                            (struct sierra_iface_info *)&typeA_interface_list;
-                        if (is_himemory(i, himemoryp)) {
-                                portdata->num_out_urbs = N_OUT_URB_HM;
-                                portdata->num_in_urbs  = N_IN_URB_HM;
-                        }
-                }
-                dev_dbg(&serial->dev->dev,
-                        "Memory usage (urbs) interface #%d, in=%d, out=%d\n",
-                        ifnum,portdata->num_in_urbs, portdata->num_out_urbs );
-                /* Set the port private data pointer */
-                usb_set_serial_port_data(port, portdata);
-        }
-
         return 0;
 }
 
 static void sierra_release(struct usb_serial *serial)
 {
-        int i;
-        struct usb_serial_port *port;
+        struct sierra_intf_private *intfdata;
+
+        intfdata = usb_get_serial_data(serial);
+        kfree(intfdata);
+}
+
+static int sierra_port_probe(struct usb_serial_port *port)
+{
+        struct usb_serial *serial = port->serial;
         struct sierra_port_private *portdata;
+        const struct sierra_iface_info *himemoryp;
+        u8 ifnum;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                port = serial->port[i];
-                if (!port)
-                        continue;
-                portdata = usb_get_serial_port_data(port);
-                if (!portdata)
-                        continue;
-                kfree(portdata);
+        portdata = kzalloc(sizeof(*portdata), GFP_KERNEL);
+        if (!portdata)
+                return -ENOMEM;
+
+        spin_lock_init(&portdata->lock);
+        init_usb_anchor(&portdata->active);
+        init_usb_anchor(&portdata->delayed);
+
+        /* Assume low memory requirements */
+        portdata->num_out_urbs = N_OUT_URB;
+        portdata->num_in_urbs  = N_IN_URB;
+
+        /* Determine actual memory requirements */
+        if (serial->num_ports == 1) {
+                /* Get interface number for composite device */
+                ifnum = sierra_calc_interface(serial);
+                himemoryp = &typeB_interface_list;
+        } else {
+                /* This is really the usb-serial port number of the interface
+                 * rather than the interface number.
+                 */
+                ifnum = port->number - serial->minor;
+                himemoryp = &typeA_interface_list;
         }
+
+        if (is_himemory(ifnum, himemoryp)) {
+                portdata->num_out_urbs = N_OUT_URB_HM;
+                portdata->num_in_urbs  = N_IN_URB_HM;
+        }
+
+        dev_dbg(&port->dev,
+                        "Memory usage (urbs) interface #%d, in=%d, out=%d\n",
+                        ifnum, portdata->num_in_urbs, portdata->num_out_urbs);
+
+        usb_set_serial_port_data(port, portdata);
+
+        return 0;
+}
+
+static int sierra_port_remove(struct usb_serial_port *port)
+{
+        struct sierra_port_private *portdata;
+
+        portdata = usb_get_serial_port_data(port);
+        kfree(portdata);
+
+        return 0;
 }
 
 #ifdef CONFIG_PM
@@ -1062,6 +1063,8 @@ static struct usb_serial_driver sierra_device = {
         .tiocmset          = sierra_tiocmset,
         .attach            = sierra_startup,
         .release           = sierra_release,
+        .port_probe        = sierra_port_probe,
+        .port_remove       = sierra_port_remove,
         .suspend           = sierra_suspend,
         .resume            = sierra_resume,
         .read_int_callback = sierra_instat_callback,
diff --git a/drivers/usb/serial/spcp8x5.c b/drivers/usb/serial/spcp8x5.c
index 9716efe92955..769c137f8975 100644
--- a/drivers/usb/serial/spcp8x5.c
+++ b/drivers/usb/serial/spcp8x5.c
@@ -157,13 +157,10 @@ struct spcp8x5_private {
         u8                      line_status;
 };
 
-/* desc : when device plug in,this function would be called.
- * thanks to usb_serial subsystem,then do almost every things for us. And what
- * we should do just alloc the buffer */
-static int spcp8x5_startup(struct usb_serial *serial)
+static int spcp8x5_port_probe(struct usb_serial_port *port)
 {
+        struct usb_serial *serial = port->serial;
         struct spcp8x5_private *priv;
-        int i;
         enum spcp8x5_type type = SPCP825_007_TYPE;
         u16 product = le16_to_cpu(serial->dev->descriptor.idProduct);
 
@@ -180,34 +177,27 @@ static int spcp8x5_startup(struct usb_serial *serial)
                 type = SPCP825_PHILIP_TYPE;
         dev_dbg(&serial->dev->dev, "device type = %d\n", (int)type);
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                priv = kzalloc(sizeof(struct spcp8x5_private), GFP_KERNEL);
-                if (!priv)
-                        goto cleanup;
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
 
-                spin_lock_init(&priv->lock);
-                init_waitqueue_head(&priv->delta_msr_wait);
-                priv->type = type;
-                usb_set_serial_port_data(serial->port[i] , priv);
-        }
+        spin_lock_init(&priv->lock);
+        init_waitqueue_head(&priv->delta_msr_wait);
+        priv->type = type;
+
+        usb_set_serial_port_data(port , priv);
 
         return 0;
-cleanup:
-        for (--i; i >= 0; --i) {
-                priv = usb_get_serial_port_data(serial->port[i]);
-                kfree(priv);
-                usb_set_serial_port_data(serial->port[i] , NULL);
-        }
-        return -ENOMEM;
 }
 
-/* call when the device plug out. free all the memory alloced by probe */
-static void spcp8x5_release(struct usb_serial *serial)
+static int spcp8x5_port_remove(struct usb_serial_port *port)
 {
-        int i;
+        struct spcp8x5_private *priv;
 
-        for (i = 0; i < serial->num_ports; i++)
-                kfree(usb_get_serial_port_data(serial->port[i]));
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 /* set the modem control line of the device.
@@ -649,8 +639,8 @@ static struct usb_serial_driver spcp8x5_device = {
         .ioctl                  = spcp8x5_ioctl,
         .tiocmget               = spcp8x5_tiocmget,
         .tiocmset               = spcp8x5_tiocmset,
-        .attach                 = spcp8x5_startup,
-        .release                = spcp8x5_release,
+        .port_probe             = spcp8x5_port_probe,
+        .port_remove            = spcp8x5_port_remove,
         .process_read_urb       = spcp8x5_process_read_urb,
 };
 
diff --git a/drivers/usb/serial/ssu100.c b/drivers/usb/serial/ssu100.c
index 015810b3785b..868d1e6852e2 100644
--- a/drivers/usb/serial/ssu100.c
+++ b/drivers/usb/serial/ssu100.c
@@ -67,13 +67,6 @@ struct ssu100_port_private {
         struct async_icount icount;
 };
 
-static void ssu100_release(struct usb_serial *serial)
-{
-        struct ssu100_port_private *priv = usb_get_serial_port_data(*serial->port);
-
-        kfree(priv);
-}
-
 static inline int ssu100_control_msg(struct usb_device *dev,
                                      u8 request, u16 data, u16 index)
 {
@@ -442,21 +435,33 @@ static int ssu100_ioctl(struct tty_struct *tty,
 
 static int ssu100_attach(struct usb_serial *serial)
 {
+        return ssu100_initdevice(serial->dev);
+}
+
+static int ssu100_port_probe(struct usb_serial_port *port)
+{
         struct ssu100_port_private *priv;
-        struct usb_serial_port *port = *serial->port;
 
         priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-        if (!priv) {
-                dev_err(&port->dev, "%s- kmalloc(%Zd) failed.\n", __func__,
-                        sizeof(*priv));
+        if (!priv)
                 return -ENOMEM;
-        }
 
         spin_lock_init(&priv->status_lock);
         init_waitqueue_head(&priv->delta_msr_wait);
+
         usb_set_serial_port_data(port, priv);
 
-        return ssu100_initdevice(serial->dev);
+        return 0;
+}
+
+static int ssu100_port_remove(struct usb_serial_port *port)
+{
+        struct ssu100_port_private *priv;
+
+        priv = usb_get_serial_port_data(port);
+        kfree(priv);
+
+        return 0;
 }
 
 static int ssu100_tiocmget(struct tty_struct *tty)
@@ -647,7 +652,8 @@ static struct usb_serial_driver ssu100_device = {
         .open                = ssu100_open,
         .close               = ssu100_close,
         .attach              = ssu100_attach,
-        .release             = ssu100_release,
+        .port_probe          = ssu100_port_probe,
+        .port_remove         = ssu100_port_remove,
         .dtr_rts             = ssu100_dtr_rts,
         .process_read_urb    = ssu100_process_read_urb,
         .tiocmget            = ssu100_tiocmget,
diff --git a/drivers/usb/serial/ti_usb_3410_5052.c b/drivers/usb/serial/ti_usb_3410_5052.c
index 6f49392cda5b..f2530d2ef3c4 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.c
+++ b/drivers/usb/serial/ti_usb_3410_5052.c
@@ -97,6 +97,8 @@ struct ti_device {
 
 static int ti_startup(struct usb_serial *serial);
 static void ti_release(struct usb_serial *serial);
+static int ti_port_probe(struct usb_serial_port *port);
+static int ti_port_remove(struct usb_serial_port *port);
 static int ti_open(struct tty_struct *tty, struct usb_serial_port *port);
 static void ti_close(struct usb_serial_port *port);
 static int ti_write(struct tty_struct *tty, struct usb_serial_port *port,
@@ -221,6 +223,8 @@ static struct usb_serial_driver ti_1port_device = {
         .num_ports              = 1,
         .attach                 = ti_startup,
         .release                = ti_release,
+        .port_probe             = ti_port_probe,
+        .port_remove            = ti_port_remove,
         .open                   = ti_open,
         .close                  = ti_close,
         .write                  = ti_write,
@@ -249,6 +253,8 @@ static struct usb_serial_driver ti_2port_device = {
         .num_ports              = 2,
         .attach                 = ti_startup,
         .release                = ti_release,
+        .port_probe             = ti_port_probe,
+        .port_remove            = ti_port_remove,
         .open                   = ti_open,
         .close                  = ti_close,
         .write                  = ti_write,
@@ -347,11 +353,8 @@ module_exit(ti_exit);
 static int ti_startup(struct usb_serial *serial)
 {
         struct ti_device *tdev;
-        struct ti_port *tport;
         struct usb_device *dev = serial->dev;
         int status;
-        int i;
-
 
         dev_dbg(&dev->dev,
                 "%s - product 0x%4X, num configurations %d, configuration value %d",
@@ -399,42 +402,8 @@ static int ti_startup(struct usb_serial *serial)
                 goto free_tdev;
         }
 
-        /* set up port structures */
-        for (i = 0; i < serial->num_ports; ++i) {
-                tport = kzalloc(sizeof(struct ti_port), GFP_KERNEL);
-                if (tport == NULL) {
-                        dev_err(&dev->dev, "%s - out of memory\n", __func__);
-                        status = -ENOMEM;
-                        goto free_tports;
-                }
-                spin_lock_init(&tport->tp_lock);
-                tport->tp_uart_base_addr = (i == 0 ?
-                                TI_UART1_BASE_ADDR : TI_UART2_BASE_ADDR);
-                tport->tp_closing_wait = closing_wait;
-                init_waitqueue_head(&tport->tp_msr_wait);
-                init_waitqueue_head(&tport->tp_write_wait);
-                if (kfifo_alloc(&tport->write_fifo, TI_WRITE_BUF_SIZE,
-                                                                GFP_KERNEL)) {
-                        dev_err(&dev->dev, "%s - out of memory\n", __func__);
-                        kfree(tport);
-                        status = -ENOMEM;
-                        goto free_tports;
-                }
-                tport->tp_port = serial->port[i];
-                tport->tp_tdev = tdev;
-                usb_set_serial_port_data(serial->port[i], tport);
-                tport->tp_uart_mode = 0;        /* default is RS232 */
-        }
-
         return 0;
 
-free_tports:
-        for (--i; i >= 0; --i) {
-                tport = usb_get_serial_port_data(serial->port[i]);
-                kfifo_free(&tport->write_fifo);
-                kfree(tport);
-                usb_set_serial_port_data(serial->port[i], NULL);
-        }
 free_tdev:
         kfree(tdev);
         usb_set_serial_data(serial, NULL);
@@ -444,21 +413,50 @@ free_tdev:
 
 static void ti_release(struct usb_serial *serial)
 {
-        int i;
         struct ti_device *tdev = usb_get_serial_data(serial);
+
+        kfree(tdev);
+}
+
+static int ti_port_probe(struct usb_serial_port *port)
+{
         struct ti_port *tport;
 
-        for (i = 0; i < serial->num_ports; ++i) {
-                tport = usb_get_serial_port_data(serial->port[i]);
-                if (tport) {
-                        kfifo_free(&tport->write_fifo);
-                        kfree(tport);
-                }
+        tport = kzalloc(sizeof(*tport), GFP_KERNEL);
+        if (!tport)
+                return -ENOMEM;
+
+        spin_lock_init(&tport->tp_lock);
+        if (port == port->serial->port[0])
+                tport->tp_uart_base_addr = TI_UART1_BASE_ADDR;
+        else
+                tport->tp_uart_base_addr = TI_UART2_BASE_ADDR;
+        tport->tp_closing_wait = closing_wait;
+        init_waitqueue_head(&tport->tp_msr_wait);
+        init_waitqueue_head(&tport->tp_write_wait);
+        if (kfifo_alloc(&tport->write_fifo, TI_WRITE_BUF_SIZE, GFP_KERNEL)) {
+                kfree(tport);
+                return -ENOMEM;
         }
+        tport->tp_port = port;
+        tport->tp_tdev = usb_get_serial_data(port->serial);
+        tport->tp_uart_mode = 0;        /* default is RS232 */
 
-        kfree(tdev);
+        usb_set_serial_port_data(port, tport);
+
+        return 0;
 }
 
+static int ti_port_remove(struct usb_serial_port *port)
+{
+        struct ti_port *tport;
+
+        tport = usb_get_serial_port_data(port);
+        kfifo_free(&tport->write_fifo);
+        kfree(tport);
+
+        return 0;
+}
 
 static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
diff --git a/drivers/usb/serial/usb-wwan.h b/drivers/usb/serial/usb-wwan.h
index 1f034d2397c6..684739b8efd0 100644
--- a/drivers/usb/serial/usb-wwan.h
+++ b/drivers/usb/serial/usb-wwan.h
@@ -8,7 +8,7 @@
 extern void usb_wwan_dtr_rts(struct usb_serial_port *port, int on);
 extern int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port);
 extern void usb_wwan_close(struct usb_serial_port *port);
-extern int usb_wwan_startup(struct usb_serial *serial);
+extern int usb_wwan_port_probe(struct usb_serial_port *port);
 extern int usb_wwan_port_remove(struct usb_serial_port *port);
 extern int usb_wwan_write_room(struct tty_struct *tty);
 extern void usb_wwan_set_termios(struct tty_struct *tty,
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index e42aa398ed37..61a73ad1a187 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -447,10 +447,12 @@ void usb_wwan_close(struct usb_serial_port *port)
 EXPORT_SYMBOL(usb_wwan_close);
 
 /* Helper functions used by usb_wwan_setup_urbs */
-static struct urb *usb_wwan_setup_urb(struct usb_serial *serial, int endpoint,
+static struct urb *usb_wwan_setup_urb(struct usb_serial_port *port,
+                                      int endpoint,
                                       int dir, void *ctx, char *buf, int len,
                                       void (*callback) (struct urb *))
 {
+        struct usb_serial *serial = port->serial;
         struct urb *urb;
 
         if (endpoint == -1)
@@ -472,101 +474,75 @@ static struct urb *usb_wwan_setup_urb(struct usb_serial *serial, int endpoint,
         return urb;
 }
 
-/* Setup urbs */
-static void usb_wwan_setup_urbs(struct usb_serial *serial)
+int usb_wwan_port_probe(struct usb_serial_port *port)
 {
-        int i, j;
-        struct usb_serial_port *port;
         struct usb_wwan_port_private *portdata;
+        struct urb *urb;
+        u8 *buffer;
+        int err;
+        int i;
 
-        for (i = 0; i < serial->num_ports; i++) {
-                port = serial->port[i];
-                portdata = usb_get_serial_port_data(port);
+        portdata = kzalloc(sizeof(*portdata), GFP_KERNEL);
+        if (!portdata)
+                return -ENOMEM;
 
-                /* Do indat endpoints first */
-                for (j = 0; j < N_IN_URB; ++j) {
-                        portdata->in_urbs[j] = usb_wwan_setup_urb(serial,
-                                                                  port->
-                                                                  bulk_in_endpointAddress,
-                                                                  USB_DIR_IN,
-                                                                  port,
-                                                                  portdata->
-                                                                  in_buffer[j],
-                                                                  IN_BUFLEN,
-                                                                  usb_wwan_indat_callback);
-                }
+        init_usb_anchor(&portdata->delayed);
 
-                /* outdat endpoints */
-                for (j = 0; j < N_OUT_URB; ++j) {
-                        portdata->out_urbs[j] = usb_wwan_setup_urb(serial,
-                                                                   port->
-                                                                   bulk_out_endpointAddress,
-                                                                   USB_DIR_OUT,
-                                                                   port,
-                                                                   portdata->
-                                                                   out_buffer
-                                                                   [j],
-                                                                   OUT_BUFLEN,
-                                                                   usb_wwan_outdat_callback);
-                }
+        for (i = 0; i < N_IN_URB; i++) {
+                buffer = (u8 *)__get_free_page(GFP_KERNEL);
+                if (!buffer)
+                        goto bail_out_error;
+                portdata->in_buffer[i] = buffer;
+
+                urb = usb_wwan_setup_urb(port, port->bulk_in_endpointAddress,
+                                                USB_DIR_IN, port,
+                                                buffer, IN_BUFLEN,
+                                                usb_wwan_indat_callback);
+                portdata->in_urbs[i] = urb;
         }
-}
-
-int usb_wwan_startup(struct usb_serial *serial)
-{
-        int i, j, err;
-        struct usb_serial_port *port;
-        struct usb_wwan_port_private *portdata;
-        u8 *buffer;
 
-        /* Now setup per port private data */
-        for (i = 0; i < serial->num_ports; i++) {
-                port = serial->port[i];
-                portdata = kzalloc(sizeof(*portdata), GFP_KERNEL);
-                if (!portdata) {
-                        dev_dbg(&port->dev, "%s: kmalloc for usb_wwan_port_private (%d) failed!.\n",
-                                __func__, i);
-                        return 1;
-                }
-                init_usb_anchor(&portdata->delayed);
+        for (i = 0; i < N_OUT_URB; i++) {
+                if (port->bulk_out_endpointAddress == -1)
+                        continue;
 
-                for (j = 0; j < N_IN_URB; j++) {
-                        buffer = (u8 *) __get_free_page(GFP_KERNEL);
-                        if (!buffer)
-                                goto bail_out_error;
-                        portdata->in_buffer[j] = buffer;
-                }
+                buffer = kmalloc(OUT_BUFLEN, GFP_KERNEL);
+                if (!buffer)
+                        goto bail_out_error2;
+                portdata->out_buffer[i] = buffer;
 
-                for (j = 0; j < N_OUT_URB; j++) {
-                        buffer = kmalloc(OUT_BUFLEN, GFP_KERNEL);
-                        if (!buffer)
-                                goto bail_out_error2;
-                        portdata->out_buffer[j] = buffer;
-                }
+                urb = usb_wwan_setup_urb(port, port->bulk_out_endpointAddress,
+                                                USB_DIR_OUT, port,
+                                                buffer, OUT_BUFLEN,
+                                                usb_wwan_outdat_callback);
+                portdata->out_urbs[i] = urb;
+        }
 
-                usb_set_serial_port_data(port, portdata);
+        usb_set_serial_port_data(port, portdata);
 
-                if (!port->interrupt_in_urb)
-                        continue;
+        if (port->interrupt_in_urb) {
                 err = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
                 if (err)
                         dev_dbg(&port->dev, "%s: submit irq_in urb failed %d\n",
                                 __func__, err);
         }
-        usb_wwan_setup_urbs(serial);
+
         return 0;
 
 bail_out_error2:
-        for (j = 0; j < N_OUT_URB; j++)
-                kfree(portdata->out_buffer[j]);
+        for (i = 0; i < N_OUT_URB; i++) {
+                usb_free_urb(portdata->out_urbs[i]);
+                kfree(portdata->out_buffer[i]);
+        }
 bail_out_error:
-        for (j = 0; j < N_IN_URB; j++)
-                if (portdata->in_buffer[j])
-                        free_page((unsigned long)portdata->in_buffer[j]);
+        for (i = 0; i < N_IN_URB; i++) {
+                usb_free_urb(portdata->in_urbs[i]);
+                free_page((unsigned long)portdata->in_buffer[i]);
+        }
         kfree(portdata);
-        return 1;
+
+        return -ENOMEM;
 }
-EXPORT_SYMBOL(usb_wwan_startup);
+EXPORT_SYMBOL_GPL(usb_wwan_port_probe);
 
 int usb_wwan_port_remove(struct usb_serial_port *port)
 {
diff --git a/drivers/usb/serial/whiteheat.c b/drivers/usb/serial/whiteheat.c
index 346c7efc20b0..b9fca3586d74 100644
--- a/drivers/usb/serial/whiteheat.c
+++ b/drivers/usb/serial/whiteheat.c
@@ -83,6 +83,8 @@ static int  whiteheat_firmware_attach(struct usb_serial *serial);
 /* function prototypes for the Connect Tech WhiteHEAT serial converter */
 static int  whiteheat_attach(struct usb_serial *serial);
 static void whiteheat_release(struct usb_serial *serial);
+static int  whiteheat_port_probe(struct usb_serial_port *port);
+static int  whiteheat_port_remove(struct usb_serial_port *port);
 static int  whiteheat_open(struct tty_struct *tty,
                         struct usb_serial_port *port);
 static void whiteheat_close(struct usb_serial_port *port);
@@ -117,6 +119,8 @@ static struct usb_serial_driver whiteheat_device = {
         .num_ports =            4,
         .attach =               whiteheat_attach,
         .release =              whiteheat_release,
+        .port_probe =           whiteheat_port_probe,
+        .port_remove =          whiteheat_port_remove,
         .open =                 whiteheat_open,
         .close =                whiteheat_close,
         .ioctl =                whiteheat_ioctl,
@@ -218,15 +222,12 @@ static int whiteheat_attach(struct usb_serial *serial)
 {
         struct usb_serial_port *command_port;
         struct whiteheat_command_private *command_info;
-        struct usb_serial_port *port;
-        struct whiteheat_private *info;
         struct whiteheat_hw_info *hw_info;
         int pipe;
         int ret;
         int alen;
         __u8 *command;
         __u8 *result;
-        int i;
 
         command_port = serial->port[COMMAND_PORT];
 
@@ -285,22 +286,6 @@ static int whiteheat_attach(struct usb_serial *serial)
                  serial->type->description,
                  hw_info->sw_major_rev, hw_info->sw_minor_rev);
 
-        for (i = 0; i < serial->num_ports; i++) {
-                port = serial->port[i];
-
-                info = kmalloc(sizeof(struct whiteheat_private), GFP_KERNEL);
-                if (info == NULL) {
-                        dev_err(&port->dev,
-                                "%s: Out of memory for port structures\n",
-                                serial->type->description);
-                        goto no_private;
-                }
-
-                info->mcr = 0;
-
-                usb_set_serial_port_data(port, info);
-        }
-
         command_info = kmalloc(sizeof(struct whiteheat_command_private),
                                                                 GFP_KERNEL);
         if (command_info == NULL) {
@@ -333,16 +318,10 @@ no_firmware:
                 "%s: please contact support@connecttech.com\n",
                 serial->type->description);
         kfree(result);
+        kfree(command);
         return -ENODEV;
 
 no_command_private:
-        for (i = serial->num_ports - 1; i >= 0; i--) {
-                port = serial->port[i];
-                info = usb_get_serial_port_data(port);
-                kfree(info);
-no_private:
-                ;
-        }
         kfree(result);
 no_result_buffer:
         kfree(command);
@@ -350,21 +329,36 @@ no_command_buffer:
         return -ENOMEM;
 }
 
-
 static void whiteheat_release(struct usb_serial *serial)
 {
         struct usb_serial_port *command_port;
-        struct whiteheat_private *info;
-        int i;
 
         /* free up our private data for our command port */
         command_port = serial->port[COMMAND_PORT];
         kfree(usb_get_serial_port_data(command_port));
+}
 
-        for (i = 0; i < serial->num_ports; i++) {
-                info = usb_get_serial_port_data(serial->port[i]);
-                kfree(info);
-        }
+static int whiteheat_port_probe(struct usb_serial_port *port)
+{
+        struct whiteheat_private *info;
+
+        info = kzalloc(sizeof(*info), GFP_KERNEL);
+        if (!info)
+                return -ENOMEM;
+
+        usb_set_serial_port_data(port, info);
+
+        return 0;
+}
+
+static int whiteheat_port_remove(struct usb_serial_port *port)
+{
+        struct whiteheat_private *info;
+
+        info = usb_get_serial_port_data(port);
+        kfree(info);
+
+        return 0;
 }
 
 static int whiteheat_open(struct tty_struct *tty, struct usb_serial_port *port)
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index 779cd954abcb..d305a5aa3a5d 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -1004,6 +1004,12 @@ UNUSUAL_DEV( 0x07cf, 0x1001, 0x1000, 0x9999,
                 USB_SC_8070, USB_PR_CB, NULL,
                 US_FL_NEED_OVERRIDE | US_FL_FIX_INQUIRY ),
 
+/* Submitted by Oleksandr Chumachenko <ledest@gmail.com> */
+UNUSUAL_DEV( 0x07cf, 0x1167, 0x0100, 0x0100,
+                "Casio",
+                "EX-N1 DigitalCamera",
+                USB_SC_8070, USB_PR_DEVICE, NULL, 0),
+
 /* Submitted by Hartmut Wahl <hwahl@hwahl.de>*/
 UNUSUAL_DEV( 0x0839, 0x000a, 0x0001, 0x0001,
                 "Samsung",
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 6d369fe9d30b..6c119944bbb6 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -408,7 +408,7 @@ static int vfio_pci_mmap(void *device_data, struct vm_area_struct *vma)
         struct vfio_pci_device *vdev = device_data;
         struct pci_dev *pdev = vdev->pdev;
         unsigned int index;
-        u64 phys_len, req_len, pgoff, req_start, phys;
+        u64 phys_len, req_len, pgoff, req_start;
         int ret;
 
         index = vma->vm_pgoff >> (VFIO_PCI_OFFSET_SHIFT - PAGE_SHIFT);
@@ -463,10 +463,9 @@ static int vfio_pci_mmap(void *device_data, struct vm_area_struct *vma)
         vma->vm_private_data = vdev;
         vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
         vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+        vma->vm_pgoff = (pci_resource_start(pdev, index) >> PAGE_SHIFT) + pgoff;
 
-        phys = (pci_resource_start(pdev, index) >> PAGE_SHIFT) + pgoff;
-
-        return remap_pfn_range(vma, vma->vm_start, phys,
+        return remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
                                req_len, vma->vm_page_prot);
 }
 
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index d8dedc7d3910..3639371fa697 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -366,6 +366,17 @@ static int vfio_intx_enable(struct vfio_pci_device *vdev)
                 return -ENOMEM;
 
         vdev->num_ctx = 1;
+
+        /*
+         * If the virtual interrupt is masked, restore it.  Devices
+         * supporting DisINTx can be masked at the hardware level
+         * here, non-PCI-2.3 devices will have to wait until the
+         * interrupt is enabled.
+         */
+        vdev->ctx[0].masked = vdev->virq_disabled;
+        if (vdev->pci_2_3)
+                pci_intx(vdev->pdev, !vdev->ctx[0].masked);
+
         vdev->irq_type = VFIO_PCI_INTX_IRQ_INDEX;
 
         return 0;
@@ -400,25 +411,26 @@ static int vfio_intx_set_signal(struct vfio_pci_device *vdev, int fd)
                 return PTR_ERR(trigger);
         }
 
+        vdev->ctx[0].trigger = trigger;
+
         if (!vdev->pci_2_3)
                 irqflags = 0;
 
         ret = request_irq(pdev->irq, vfio_intx_handler,
                           irqflags, vdev->ctx[0].name, vdev);
         if (ret) {
+                vdev->ctx[0].trigger = NULL;
                 kfree(vdev->ctx[0].name);
                 eventfd_ctx_put(trigger);
                 return ret;
         }
 
-        vdev->ctx[0].trigger = trigger;
-
         /*
          * INTx disable will stick across the new irq setup,
          * disable_irq won't.
          */
         spin_lock_irqsave(&vdev->irqlock, flags);
-        if (!vdev->pci_2_3 && (vdev->ctx[0].masked || vdev->virq_disabled))
+        if (!vdev->pci_2_3 && vdev->ctx[0].masked)
                 disable_irq_nosync(pdev->irq);
         spin_unlock_irqrestore(&vdev->irqlock, flags);
 
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index 072cbbadbc36..7f93f34b7f91 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -379,7 +379,8 @@ static void handle_rx(struct vhost_net *net)
                 .hdr.gso_type = VIRTIO_NET_HDR_GSO_NONE
         };
         size_t total_len = 0;
-        int err, headcount, mergeable;
+        int err, mergeable;
+        s16 headcount;
         size_t vhost_hlen, sock_hlen;
         size_t vhost_len, sock_len;
         /* TODO: check that we are running from vhost_worker? */
diff --git a/drivers/vhost/tcm_vhost.c b/drivers/vhost/tcm_vhost.c
index ed8e2e6c8df2..aa31692064dd 100644
--- a/drivers/vhost/tcm_vhost.c
+++ b/drivers/vhost/tcm_vhost.c
@@ -330,17 +330,6 @@ static int tcm_vhost_queue_tm_rsp(struct se_cmd *se_cmd)
         return 0;
 }
 
-static u16 tcm_vhost_set_fabric_sense_len(struct se_cmd *se_cmd,
-        u32 sense_length)
-{
-        return 0;
-}
-
-static u16 tcm_vhost_get_fabric_sense_len(void)
-{
-        return 0;
-}
-
 static void vhost_scsi_free_cmd(struct tcm_vhost_cmd *tv_cmd)
 {
         struct se_cmd *se_cmd = &tv_cmd->tvc_se_cmd;
@@ -426,10 +415,7 @@ static struct tcm_vhost_cmd *vhost_scsi_allocate_cmd(
 {
         struct tcm_vhost_cmd *tv_cmd;
         struct tcm_vhost_nexus *tv_nexus;
-        struct se_portal_group *se_tpg = &tv_tpg->se_tpg;
         struct se_session *se_sess;
-        struct se_cmd *se_cmd;
-        int sam_task_attr;
 
         tv_nexus = tv_tpg->tpg_nexus;
         if (!tv_nexus) {
@@ -445,23 +431,11 @@ static struct tcm_vhost_cmd *vhost_scsi_allocate_cmd(
         }
         INIT_LIST_HEAD(&tv_cmd->tvc_completion_list);
         tv_cmd->tvc_tag = v_req->tag;
+        tv_cmd->tvc_task_attr = v_req->task_attr;
+        tv_cmd->tvc_exp_data_len = exp_data_len;
+        tv_cmd->tvc_data_direction = data_direction;
+        tv_cmd->tvc_nexus = tv_nexus;
 
-        se_cmd = &tv_cmd->tvc_se_cmd;
-        /*
-         * Locate the SAM Task Attr from virtio_scsi_cmd_req
-         */
-        sam_task_attr = v_req->task_attr;
-        /*
-         * Initialize struct se_cmd descriptor from TCM infrastructure
-         */
-        transport_init_se_cmd(se_cmd, se_tpg->se_tpg_tfo, se_sess, exp_data_len,
-                                data_direction, sam_task_attr,
-                                &tv_cmd->tvc_sense_buf[0]);
-
-#if 0   /* FIXME: vhost_scsi_allocate_cmd() BIDI operation */
-        if (bidi)
-                se_cmd->se_cmd_flags |= SCF_BIDI;
-#endif
         return tv_cmd;
 }
 
@@ -560,37 +534,10 @@ static void tcm_vhost_submission_work(struct work_struct *work)
 {
         struct tcm_vhost_cmd *tv_cmd =
                 container_of(work, struct tcm_vhost_cmd, work);
+        struct tcm_vhost_nexus *tv_nexus;
         struct se_cmd *se_cmd = &tv_cmd->tvc_se_cmd;
         struct scatterlist *sg_ptr, *sg_bidi_ptr = NULL;
         int rc, sg_no_bidi = 0;
-        /*
-         * Locate the struct se_lun pointer based on v_req->lun, and
-         * attach it to struct se_cmd
-         */
-        rc = transport_lookup_cmd_lun(&tv_cmd->tvc_se_cmd, tv_cmd->tvc_lun);
-        if (rc < 0) {
-                pr_err("Failed to look up lun: %d\n", tv_cmd->tvc_lun);
-                transport_send_check_condition_and_sense(&tv_cmd->tvc_se_cmd,
-                        tv_cmd->tvc_se_cmd.scsi_sense_reason, 0);
-                transport_generic_free_cmd(se_cmd, 0);
-                return;
-        }
-
-        rc = target_setup_cmd_from_cdb(se_cmd, tv_cmd->tvc_cdb);
-        if (rc == -ENOMEM) {
-                transport_send_check_condition_and_sense(se_cmd,
-                                TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
-                transport_generic_free_cmd(se_cmd, 0);
-                return;
-        } else if (rc < 0) {
-                if (se_cmd->se_cmd_flags & SCF_SCSI_RESERVATION_CONFLICT)
-                        tcm_vhost_queue_status(se_cmd);
-                else
-                        transport_send_check_condition_and_sense(se_cmd,
-                                        se_cmd->scsi_sense_reason, 0);
-                transport_generic_free_cmd(se_cmd, 0);
-                return;
-        }
 
         if (tv_cmd->tvc_sgl_count) {
                 sg_ptr = tv_cmd->tvc_sgl;
@@ -608,17 +555,19 @@ static void tcm_vhost_submission_work(struct work_struct *work)
         } else {
                 sg_ptr = NULL;
         }
-
-        rc = transport_generic_map_mem_to_cmd(se_cmd, sg_ptr,
-                                tv_cmd->tvc_sgl_count, sg_bidi_ptr,
-                                sg_no_bidi);
+        tv_nexus = tv_cmd->tvc_nexus;
+
+        rc = target_submit_cmd_map_sgls(se_cmd, tv_nexus->tvn_se_sess,
+                        tv_cmd->tvc_cdb, &tv_cmd->tvc_sense_buf[0],
+                        tv_cmd->tvc_lun, tv_cmd->tvc_exp_data_len,
+                        tv_cmd->tvc_task_attr, tv_cmd->tvc_data_direction,
+                        0, sg_ptr, tv_cmd->tvc_sgl_count,
+                        sg_bidi_ptr, sg_no_bidi);
         if (rc < 0) {
                 transport_send_check_condition_and_sense(se_cmd,
-                                se_cmd->scsi_sense_reason, 0);
+                                TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
                 transport_generic_free_cmd(se_cmd, 0);
-                return;
         }
-        transport_handle_cdb_direct(se_cmd);
 }
 
 static void vhost_scsi_handle_vq(struct vhost_scsi *vs)
@@ -1531,8 +1480,6 @@ static struct target_core_fabric_ops tcm_vhost_ops = {
         .queue_data_in                  = tcm_vhost_queue_data_in,
         .queue_status                   = tcm_vhost_queue_status,
         .queue_tm_rsp                   = tcm_vhost_queue_tm_rsp,
-        .get_fabric_sense_len           = tcm_vhost_get_fabric_sense_len,
-        .set_fabric_sense_len           = tcm_vhost_set_fabric_sense_len,
         /*
          * Setup callers for generic logic in target_core_fabric_configfs.c
          */
diff --git a/drivers/vhost/tcm_vhost.h b/drivers/vhost/tcm_vhost.h
index d9e93557d669..7e87c63ecbcd 100644
--- a/drivers/vhost/tcm_vhost.h
+++ b/drivers/vhost/tcm_vhost.h
@@ -5,6 +5,12 @@
 struct tcm_vhost_cmd {
         /* Descriptor from vhost_get_vq_desc() for virt_queue segment */
         int tvc_vq_desc;
+        /* virtio-scsi initiator task attribute */
+        int tvc_task_attr;
+        /* virtio-scsi initiator data direction */
+        enum dma_data_direction tvc_data_direction;
+        /* Expected data transfer length from virtio-scsi header */
+        u32 tvc_exp_data_len;
         /* The Tag from include/linux/virtio_scsi.h:struct virtio_scsi_cmd_req */
         u64 tvc_tag;
         /* The number of scatterlists associated with this cmd */
@@ -17,6 +23,8 @@ struct tcm_vhost_cmd {
         struct virtio_scsi_cmd_resp __user *tvc_resp;
         /* Pointer to vhost_scsi for our device */
         struct vhost_scsi *tvc_vhost;
+        /* Pointer to vhost nexus memory */
+        struct tcm_vhost_nexus *tvc_nexus;
         /* The TCM I/O descriptor that is accessed via container_of() */
         struct se_cmd tvc_se_cmd;
         /* work item used for cmwq dispatch to tcm_vhost_submission_work() */
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 20c33c42600a..d08d7998a4aa 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -2139,21 +2139,6 @@ config FB_UDL
           mplayer -vo fbdev. Supports all USB 2.0 era DisplayLink devices.
           To compile as a module, choose M here: the module name is udlfb.
 
-config FB_PNX4008_DUM
-        tristate "Display Update Module support on Philips PNX4008 board"
-        depends on FB && ARCH_PNX4008
-        ---help---
-          Say Y here to enable support for PNX4008 Display Update Module (DUM)
-
-config FB_PNX4008_DUM_RGB
-        tristate "RGB Framebuffer support on Philips PNX4008 board"
-        depends on FB_PNX4008_DUM
-        select FB_CFB_FILLRECT
-        select FB_CFB_COPYAREA
-        select FB_CFB_IMAGEBLIT
-        ---help---
-          Say Y here to enable support for PNX4008 RGB Framebuffer
-
 config FB_IBM_GXT4500
         tristate "Framebuffer support for IBM GXT4500P adaptor"
         depends on FB && PPC
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 194035986af2..23e948ebfab8 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -127,8 +127,6 @@ obj-$(CONFIG_FB_S3C)		  += s3c-fb.o
 obj-$(CONFIG_FB_S3C2410)          += s3c2410fb.o
 obj-$(CONFIG_FB_FSL_DIU)          += fsl-diu-fb.o
 obj-$(CONFIG_FB_COBALT)           += cobalt_lcdfb.o
-obj-$(CONFIG_FB_PNX4008_DUM)      += pnx4008/
-obj-$(CONFIG_FB_PNX4008_DUM_RGB)  += pnx4008/
 obj-$(CONFIG_FB_IBM_GXT4500)      += gxt4500.o
 obj-$(CONFIG_FB_PS3)              += ps3fb.o
 obj-$(CONFIG_FB_SM501)            += sm501fb.o
diff --git a/drivers/video/amifb.c b/drivers/video/amifb.c
index 887df9d81422..7fa1bf823729 100644
--- a/drivers/video/amifb.c
+++ b/drivers/video/amifb.c
@@ -949,7 +949,6 @@ static int round_down_bpp = 1;	/* for mode probing */
 
 
 static int amifb_ilbm = 0;      /* interleaved or normal bitplanes */
-static int amifb_inverse = 0;
 
 static u32 amifb_hfmin __initdata;      /* monitor hfreq lower limit (Hz) */
 static u32 amifb_hfmax __initdata;      /* monitor hfreq upper limit (Hz) */
@@ -2355,7 +2354,6 @@ static int __init amifb_setup(char *options)
                 if (!*this_opt)
                         continue;
                 if (!strcmp(this_opt, "inverse")) {
-                        amifb_inverse = 1;
                         fb_invert_cmaps();
                 } else if (!strcmp(this_opt, "ilbm"))
                         amifb_ilbm = 1;
diff --git a/drivers/video/arcfb.c b/drivers/video/arcfb.c
index a1d58e9d3073..4659d5da6ff8 100644
--- a/drivers/video/arcfb.c
+++ b/drivers/video/arcfb.c
@@ -552,6 +552,7 @@ static int __devinit arcfb_probe(struct platform_device *dev)
                                 "arcfb", info)) {
                         printk(KERN_INFO
                                 "arcfb: Failed req IRQ %d\n", par->irq);
+                        retval = -EBUSY;
                         goto err1;
                 }
         }
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 15055395cd95..94cac9f9919f 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -931,8 +931,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 }
 
                 info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
-                if (!info->screen_base)
+                if (!info->screen_base) {
+                        ret = -ENOMEM;
                         goto release_intmem;
+                }
 
                 /*
                  * Don't clear the framebuffer -- someone may have set
@@ -960,6 +962,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
         sinfo->mmio = ioremap(info->fix.mmio_start, info->fix.mmio_len);
         if (!sinfo->mmio) {
                 dev_err(dev, "cannot map LCDC registers\n");
+                ret = -ENOMEM;
                 goto release_mem;
         }
 
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index c101697a4ba7..765a945f8ea1 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -60,7 +60,8 @@ config LCD_LTV350QV
           The LTV350QV panel is present on all ATSTK1000 boards.
 
 config LCD_ILI9320
-        tristate
+        tristate "ILI Technology ILI9320 controller support"
+        depends on SPI
         help
           If you have a panel based on the ILI9320 controller chip
           then say y to include a power driver for it.
diff --git a/drivers/video/backlight/lm3639_bl.c b/drivers/video/backlight/lm3639_bl.c
index c6915c6c3cd1..585949b57055 100644
--- a/drivers/video/backlight/lm3639_bl.c
+++ b/drivers/video/backlight/lm3639_bl.c
@@ -206,11 +206,11 @@ static ssize_t lm3639_bled_mode_store(struct device *dev,
 
 out:
         dev_err(pchip->dev, "%s:i2c access fail to register\n", __func__);
-        return size;
+        return ret;
 
 out_input:
         dev_err(pchip->dev, "%s:input conversion fail\n", __func__);
-        return size;
+        return ret;
 
 }
 
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 995f0164c9b0..069983ca49ff 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -213,7 +213,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
         pb->exit = data->exit;
         pb->dev = &pdev->dev;
 
-        pb->pwm = pwm_get(&pdev->dev, NULL);
+        pb->pwm = devm_pwm_get(&pdev->dev, NULL);
         if (IS_ERR(pb->pwm)) {
                 dev_err(&pdev->dev, "unable to request PWM, trying legacy API\n");
 
@@ -246,7 +246,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
         if (IS_ERR(bl)) {
                 dev_err(&pdev->dev, "failed to register backlight\n");
                 ret = PTR_ERR(bl);
-                goto err_bl;
+                goto err_alloc;
         }
 
         bl->props.brightness = data->dft_brightness;
@@ -255,8 +255,6 @@ static int pwm_backlight_probe(struct platform_device *pdev)
         platform_set_drvdata(pdev, bl);
         return 0;
 
-err_bl:
-        pwm_put(pb->pwm);
 err_alloc:
         if (data->exit)
                 data->exit(&pdev->dev);
@@ -271,7 +269,6 @@ static int pwm_backlight_remove(struct platform_device *pdev)
         backlight_device_unregister(bl);
         pwm_config(pb->pwm, 0, pb->period);
         pwm_disable(pb->pwm);
-        pwm_put(pb->pwm);
         if (pb->exit)
                 pb->exit(&pdev->dev);
         return 0;
diff --git a/drivers/video/bf537-lq035.c b/drivers/video/bf537-lq035.c
index befbc80d11fc..7347aa1e5e4a 100644
--- a/drivers/video/bf537-lq035.c
+++ b/drivers/video/bf537-lq035.c
@@ -760,18 +760,20 @@ static int __devinit bfin_lq035_probe(struct platform_device *pdev)
         bfin_lq035_fb.flags = FBINFO_DEFAULT;
 
 
-        bfin_lq035_fb.pseudo_palette = kzalloc(sizeof(u32) * 16, GFP_KERNEL);
+        bfin_lq035_fb.pseudo_palette = devm_kzalloc(&pdev->dev,
+                                                    sizeof(u32) * 16,
+                                                    GFP_KERNEL);
         if (bfin_lq035_fb.pseudo_palette == NULL) {
                 pr_err("failed to allocate pseudo_palette\n");
                 ret = -ENOMEM;
-                goto out_palette;
+                goto out_table;
         }
 
         if (fb_alloc_cmap(&bfin_lq035_fb.cmap, NBR_PALETTE, 0) < 0) {
                 pr_err("failed to allocate colormap (%d entries)\n",
                         NBR_PALETTE);
                 ret = -EFAULT;
-                goto out_cmap;
+                goto out_table;
         }
 
         if (register_framebuffer(&bfin_lq035_fb) < 0) {
@@ -804,9 +806,6 @@ out_lcd:
         unregister_framebuffer(&bfin_lq035_fb);
 out_reg:
         fb_dealloc_cmap(&bfin_lq035_fb.cmap);
-out_cmap:
-        kfree(bfin_lq035_fb.pseudo_palette);
-out_palette:
 out_table:
         dma_free_coherent(NULL, TOTAL_VIDEO_MEM_SIZE, fb_buffer, 0);
         fb_buffer = NULL;
@@ -834,7 +833,6 @@ static int __devexit bfin_lq035_remove(struct platform_device *pdev)
         free_dma(CH_PPI);
 
 
-        kfree(bfin_lq035_fb.pseudo_palette);
         fb_dealloc_cmap(&bfin_lq035_fb.cmap);
 
 
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index dc2f0047769b..ff5663f5c64f 100644
--- a/drivers/video/bf54x-lq043fb.c
+++ b/drivers/video/bf54x-lq043fb.c
@@ -525,6 +525,7 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
         info = fbinfo->par;
         info->fb = fbinfo;
         info->dev = &pdev->dev;
+        spin_lock_init(&info->lock);
 
         platform_set_drvdata(pdev, fbinfo);
 
@@ -601,7 +602,8 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 
         fbinfo->fbops = &bfin_bf54x_fb_ops;
 
-        fbinfo->pseudo_palette = kzalloc(sizeof(u32) * 16, GFP_KERNEL);
+        fbinfo->pseudo_palette = devm_kzalloc(&pdev->dev, sizeof(u32) * 16,
+                                              GFP_KERNEL);
         if (!fbinfo->pseudo_palette) {
                 printk(KERN_ERR DRIVER_NAME
                        "Fail to allocate pseudo_palette\n");
@@ -616,7 +618,7 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
                        "Fail to allocate colormap (%d entries)\n",
                        BFIN_LCD_NBR_PALETTE_ENTRIES);
                 ret = -EFAULT;
-                goto out5;
+                goto out4;
         }
 
         if (request_ports(info)) {
@@ -671,8 +673,6 @@ out7:
         free_ports(info);
 out6:
         fb_dealloc_cmap(&fbinfo->cmap);
-out5:
-        kfree(fbinfo->pseudo_palette);
 out4:
         dma_free_coherent(NULL, fbinfo->fix.smem_len, info->fb_buffer,
                           info->dma_handle);
@@ -699,7 +699,6 @@ static int __devexit bfin_bf54x_remove(struct platform_device *pdev)
                 dma_free_coherent(NULL, fbinfo->fix.smem_len, info->fb_buffer,
                                   info->dma_handle);
 
-        kfree(fbinfo->pseudo_palette);
         fb_dealloc_cmap(&fbinfo->cmap);
 
 #ifndef NO_BL_SUPPORT
diff --git a/drivers/video/bfin-lq035q1-fb.c b/drivers/video/bfin-lq035q1-fb.c
index 353c02fe8a95..6fbc75c2f0a1 100644
--- a/drivers/video/bfin-lq035q1-fb.c
+++ b/drivers/video/bfin-lq035q1-fb.c
@@ -577,6 +577,7 @@ static int __devinit bfin_lq035q1_probe(struct platform_device *pdev)
         info = fbinfo->par;
         info->fb = fbinfo;
         info->dev = &pdev->dev;
+        spin_lock_init(&info->lock);
 
         info->disp_info = pdev->dev.platform_data;
 
@@ -853,17 +854,7 @@ static struct platform_driver bfin_lq035q1_driver = {
         },
 };
 
-static int __init bfin_lq035q1_driver_init(void)
-{
-        return platform_driver_register(&bfin_lq035q1_driver);
-}
-module_init(bfin_lq035q1_driver_init);
-
-static void __exit bfin_lq035q1_driver_cleanup(void)
-{
-        platform_driver_unregister(&bfin_lq035q1_driver);
-}
-module_exit(bfin_lq035q1_driver_cleanup);
+module_platform_driver(bfin_lq035q1_driver);
 
 MODULE_DESCRIPTION("Blackfin TFT LCD Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 7a0c05f3537e..ae0fb24b8b43 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -447,6 +447,7 @@ static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
         info = fbinfo->par;
         info->fb = fbinfo;
         info->dev = &pdev->dev;
+        spin_lock_init(&info->lock);
 
         platform_set_drvdata(pdev, fbinfo);
 
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 7ba74cd4be61..6bea9a936798 100644
--- a/drivers/video/bw2.c
+++ b/drivers/video/bw2.c
@@ -319,8 +319,10 @@ static int __devinit bw2_probe(struct platform_device *op)
 
         info->screen_base = of_ioremap(&op->resource[0], 0,
                                        info->fix.smem_len, "bw2 ram");
-        if (!info->screen_base)
+        if (!info->screen_base) {
+                err = -ENOMEM;
                 goto out_unmap_regs;
+        }
 
         bw2_blank(FB_BLANK_UNBLANK, info);
 
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index f927a7b1a8d4..c5e7612ff876 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -398,7 +398,8 @@ static int __devinit cg3_probe(struct platform_device *op)
                         goto out_unmap_screen;
         }
 
-        if (fb_alloc_cmap(&info->cmap, 256, 0))
+        err = fb_alloc_cmap(&info->cmap, 256, 0);
+        if (err)
                 goto out_unmap_screen;
 
         fb_set_cmap(&info->cmap, info);
diff --git a/drivers/video/cobalt_lcdfb.c b/drivers/video/cobalt_lcdfb.c
index eae46f6457e2..01a4ee7cc6b1 100644
--- a/drivers/video/cobalt_lcdfb.c
+++ b/drivers/video/cobalt_lcdfb.c
@@ -348,7 +348,8 @@ static int __devinit cobalt_lcdfb_probe(struct platform_device *dev)
         }
 
         info->screen_size = resource_size(res);
-        info->screen_base = ioremap(res->start, info->screen_size);
+        info->screen_base = devm_ioremap(&dev->dev, res->start,
+                                         info->screen_size);
         info->fbops = &cobalt_lcd_fbops;
         info->fix = cobalt_lcdfb_fix;
         info->fix.smem_start = res->start;
@@ -359,7 +360,6 @@ static int __devinit cobalt_lcdfb_probe(struct platform_device *dev)
 
         retval = register_framebuffer(info);
         if (retval < 0) {
-                iounmap(info->screen_base);
                 framebuffer_release(info);
                 return retval;
         }
@@ -380,7 +380,6 @@ static int __devexit cobalt_lcdfb_remove(struct platform_device *dev)
 
         info = platform_get_drvdata(dev);
         if (info) {
-                iounmap(info->screen_base);
                 unregister_framebuffer(info);
                 framebuffer_release(info);
         }
diff --git a/drivers/video/console/font_mini_4x6.c b/drivers/video/console/font_mini_4x6.c
index fa6e698e63c4..838caa1cfef7 100644
--- a/drivers/video/console/font_mini_4x6.c
+++ b/drivers/video/console/font_mini_4x6.c
@@ -1092,7 +1092,7 @@ static const unsigned char fontdata_mini_4x6[FONTDATAMAX] = {
         /*{*/   /*   Char 124: '|' */
         0x44,   /*=   [ *  ]        */
         0x44,   /*=   [ *  ]        */
-        0x00,   /*=   [    ]        */
+        0x44,   /*=   [ *  ]        */
         0x44,   /*=   [ *  ]        */
         0x44,   /*=   [ *  ]        */
         0x00,   /*=   [    ]        */
diff --git a/drivers/video/console/font_sun8x16.c b/drivers/video/console/font_sun8x16.c
index 5abf290c6eb7..268151325b83 100644
--- a/drivers/video/console/font_sun8x16.c
+++ b/drivers/video/console/font_sun8x16.c
@@ -127,7 +127,7 @@ static const unsigned char fontdata_sun8x16[FONTDATAMAX] = {
 /*y*/ 0x00,0x00,0x00,0x00,0x00,0xc6,0xc6,0xc6,0xc6,0xc6,0xc6,0x7e,0x06,0x0c,0xf8,0x00,
 /*z*/ 0x00,0x00,0x00,0x00,0x00,0xfe,0xcc,0x18,0x30,0x60,0xc6,0xfe,0x00,0x00,0x00,0x00,
 /*{*/ 0x00,0x00,0x0e,0x18,0x18,0x18,0x70,0x18,0x18,0x18,0x18,0x0e,0x00,0x00,0x00,0x00,
-/*|*/ 0x00,0x00,0x18,0x18,0x18,0x18,0x00,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,
+/*|*/ 0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,
 /*}*/ 0x00,0x00,0x70,0x18,0x18,0x18,0x0e,0x18,0x18,0x18,0x18,0x70,0x00,0x00,0x00,0x00,
 /*~*/ 0x00,0x00,0x76,0xdc,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
 /* */ 0x00,0x00,0x00,0x00,0x10,0x38,0x6c,0xc6,0xc6,0xc6,0xfe,0x00,0x00,0x00,0x00,0x00,
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index c1527f5b47ee..e40125cb313e 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -1804,8 +1804,10 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
         cfb->irq = dev->irq;
         cfb->region = pci_ioremap_bar(dev, 0);
-        if (!cfb->region)
+        if (!cfb->region) {
+                err = -ENOMEM;
                 goto failed_ioremap;
+        }
 
         cfb->regs = cfb->region + MMIO_OFFSET;
         cfb->fb.device = &dev->dev;
diff --git a/drivers/video/da8xx-fb.c b/drivers/video/da8xx-fb.c
index 113d43a16f54..80665f66ac1a 100644
--- a/drivers/video/da8xx-fb.c
+++ b/drivers/video/da8xx-fb.c
@@ -26,7 +26,9 @@
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
+#include <linux/pm_runtime.h>
 #include <linux/interrupt.h>
+#include <linux/wait.h>
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
 #include <linux/console.h>
@@ -48,6 +50,7 @@
 #define LCD_PL_LOAD_DONE                BIT(6)
 #define LCD_FIFO_UNDERFLOW              BIT(5)
 #define LCD_SYNC_LOST                   BIT(2)
+#define LCD_FRAME_DONE                  BIT(0)
 
 /* LCD DMA Control Register */
 #define LCD_DMA_BURST_SIZE(x)           ((x) << 4)
@@ -86,6 +89,8 @@
 #define LCD_V2_LIDD_CLK_EN              BIT(1)
 #define LCD_V2_CORE_CLK_EN              BIT(0)
 #define LCD_V2_LPP_B10                  26
+#define LCD_V2_TFT_24BPP_MODE           BIT(25)
+#define LCD_V2_TFT_24BPP_UNPACK         BIT(26)
 
 /* LCD Raster Timing 2 Register */
 #define LCD_AC_BIAS_TRANSITIONS_PER_INT(x)      ((x) << 16)
@@ -135,6 +140,8 @@ static void __iomem *da8xx_fb_reg_base;
 static struct resource *lcdc_regs;
 static unsigned int lcd_revision;
 static irq_handler_t lcdc_irq_handler;
+static wait_queue_head_t frame_done_wq;
+static int frame_done_flag;
 
 static inline unsigned int lcdc_read(unsigned int addr)
 {
@@ -156,7 +163,6 @@ struct da8xx_fb_par {
         unsigned int            dma_end;
         struct clk *lcdc_clk;
         int irq;
-        unsigned short pseudo_palette[16];
         unsigned int palette_sz;
         unsigned int pxl_clk;
         int blank;
@@ -175,6 +181,7 @@ struct da8xx_fb_par {
         unsigned int            lcd_fck_rate;
 #endif
         void (*panel_power_ctrl)(int);
+        u32 pseudo_palette[16];
 };
 
 /* Variable Screen Information */
@@ -288,13 +295,26 @@ static inline void lcd_enable_raster(void)
 }
 
 /* Disable the Raster Engine of the LCD Controller */
-static inline void lcd_disable_raster(void)
+static inline void lcd_disable_raster(bool wait_for_frame_done)
 {
         u32 reg;
+        int ret;
 
         reg = lcdc_read(LCD_RASTER_CTRL_REG);
         if (reg & LCD_RASTER_ENABLE)
                 lcdc_write(reg & ~LCD_RASTER_ENABLE, LCD_RASTER_CTRL_REG);
+        else
+                /* return if already disabled */
+                return;
+
+        if ((wait_for_frame_done == true) && (lcd_revision == LCD_VERSION_2)) {
+                frame_done_flag = 0;
+                ret = wait_event_interruptible_timeout(frame_done_wq,
+                                frame_done_flag != 0,
+                                msecs_to_jiffies(50));
+                if (ret == 0)
+                        pr_err("LCD Controller timed out\n");
+        }
 }
 
 static void lcd_blit(int load_mode, struct da8xx_fb_par *par)
@@ -321,7 +341,8 @@ static void lcd_blit(int load_mode, struct da8xx_fb_par *par)
                 } else {
                         reg_int = lcdc_read(LCD_INT_ENABLE_SET_REG) |
                                 LCD_V2_END_OF_FRAME0_INT_ENA |
-                                LCD_V2_END_OF_FRAME1_INT_ENA;
+                                LCD_V2_END_OF_FRAME1_INT_ENA |
+                                LCD_FRAME_DONE;
                         lcdc_write(reg_int, LCD_INT_ENABLE_SET_REG);
                 }
                 reg_dma |= LCD_DUAL_FRAME_BUFFER_ENABLE;
@@ -499,6 +520,9 @@ static int lcd_cfg_frame_buffer(struct da8xx_fb_par *par, u32 width, u32 height,
 {
         u32 reg;
 
+        if (bpp > 16 && lcd_revision == LCD_VERSION_1)
+                return -EINVAL;
+
         /* Set the Panel Width */
         /* Pixels per line = (PPL + 1)*16 */
         if (lcd_revision == LCD_VERSION_1) {
@@ -542,14 +566,19 @@ static int lcd_cfg_frame_buffer(struct da8xx_fb_par *par, u32 width, u32 height,
         reg = lcdc_read(LCD_RASTER_CTRL_REG) & ~(1 << 8);
         if (raster_order)
                 reg |= LCD_RASTER_ORDER;
-        lcdc_write(reg, LCD_RASTER_CTRL_REG);
+
+        par->palette_sz = 16 * 2;
 
         switch (bpp) {
         case 1:
         case 2:
         case 4:
         case 16:
-                par->palette_sz = 16 * 2;
+                break;
+        case 24:
+                reg |= LCD_V2_TFT_24BPP_MODE;
+        case 32:
+                reg |= LCD_V2_TFT_24BPP_UNPACK;
                 break;
 
         case 8:
@@ -560,9 +589,12 @@ static int lcd_cfg_frame_buffer(struct da8xx_fb_par *par, u32 width, u32 height,
                 return -EINVAL;
         }
 
+        lcdc_write(reg, LCD_RASTER_CTRL_REG);
+
         return 0;
 }
 
+#define CNVT_TOHW(val, width) ((((val) << (width)) + 0x7FFF - (val)) >> 16)
 static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
                               unsigned blue, unsigned transp,
                               struct fb_info *info)
@@ -578,13 +610,38 @@ static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
         if (info->fix.visual == FB_VISUAL_DIRECTCOLOR)
                 return 1;
 
-        if (info->var.bits_per_pixel == 4) {
-                if (regno > 15)
-                        return 1;
+        if (info->var.bits_per_pixel > 16 && lcd_revision == LCD_VERSION_1)
+                return -EINVAL;
 
-                if (info->var.grayscale) {
-                        pal = regno;
-                } else {
+        switch (info->fix.visual) {
+        case FB_VISUAL_TRUECOLOR:
+                red = CNVT_TOHW(red, info->var.red.length);
+                green = CNVT_TOHW(green, info->var.green.length);
+                blue = CNVT_TOHW(blue, info->var.blue.length);
+                break;
+        case FB_VISUAL_PSEUDOCOLOR:
+                switch (info->var.bits_per_pixel) {
+                case 4:
+                        if (regno > 15)
+                                return -EINVAL;
+
+                        if (info->var.grayscale) {
+                                pal = regno;
+                        } else {
+                                red >>= 4;
+                                green >>= 8;
+                                blue >>= 12;
+
+                                pal = red & 0x0f00;
+                                pal |= green & 0x00f0;
+                                pal |= blue & 0x000f;
+                        }
+                        if (regno == 0)
+                                pal |= 0x2000;
+                        palette[regno] = pal;
+                        break;
+
+                case 8:
                         red >>= 4;
                         green >>= 8;
                         blue >>= 12;
@@ -592,36 +649,36 @@ static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
                         pal = (red & 0x0f00);
                         pal |= (green & 0x00f0);
                         pal |= (blue & 0x000f);
-                }
-                if (regno == 0)
-                        pal |= 0x2000;
-                palette[regno] = pal;
-
-        } else if (info->var.bits_per_pixel == 8) {
-                red >>= 4;
-                green >>= 8;
-                blue >>= 12;
-
-                pal = (red & 0x0f00);
-                pal |= (green & 0x00f0);
-                pal |= (blue & 0x000f);
 
-                if (palette[regno] != pal) {
-                        update_hw = 1;
-                        palette[regno] = pal;
+                        if (palette[regno] != pal) {
+                                update_hw = 1;
+                                palette[regno] = pal;
+                        }
+                        break;
                 }
-        } else if ((info->var.bits_per_pixel == 16) && regno < 16) {
-                red >>= (16 - info->var.red.length);
-                red <<= info->var.red.offset;
+                break;
+        }
 
-                green >>= (16 - info->var.green.length);
-                green <<= info->var.green.offset;
+        /* Truecolor has hardware independent palette */
+        if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+                u32 v;
 
-                blue >>= (16 - info->var.blue.length);
-                blue <<= info->var.blue.offset;
+                if (regno > 15)
+                        return -EINVAL;
 
-                par->pseudo_palette[regno] = red | green | blue;
+                v = (red << info->var.red.offset) |
+                        (green << info->var.green.offset) |
+                        (blue << info->var.blue.offset);
 
+                switch (info->var.bits_per_pixel) {
+                case 16:
+                        ((u16 *) (info->pseudo_palette))[regno] = v;
+                        break;
+                case 24:
+                case 32:
+                        ((u32 *) (info->pseudo_palette))[regno] = v;
+                        break;
+                }
                 if (palette[0] != 0x4000) {
                         update_hw = 1;
                         palette[0] = 0x4000;
@@ -634,11 +691,12 @@ static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 
         return 0;
 }
+#undef CNVT_TOHW
 
 static void lcd_reset(struct da8xx_fb_par *par)
 {
         /* Disable the Raster if previously Enabled */
-        lcd_disable_raster();
+        lcd_disable_raster(false);
 
         /* DMA has to be disabled */
         lcdc_write(0, LCD_DMA_CTRL_REG);
@@ -734,7 +792,7 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
         u32 stat = lcdc_read(LCD_MASKED_STAT_REG);
 
         if ((stat & LCD_SYNC_LOST) && (stat & LCD_FIFO_UNDERFLOW)) {
-                lcd_disable_raster();
+                lcd_disable_raster(false);
                 lcdc_write(stat, LCD_MASKED_STAT_REG);
                 lcd_enable_raster();
         } else if (stat & LCD_PL_LOAD_DONE) {
@@ -744,7 +802,7 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
                  * interrupt via the following write to the status register. If
                  * this is done after then one gets multiple PL done interrupts.
                  */
-                lcd_disable_raster();
+                lcd_disable_raster(false);
 
                 lcdc_write(stat, LCD_MASKED_STAT_REG);
 
@@ -775,6 +833,14 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
                         par->vsync_flag = 1;
                         wake_up_interruptible(&par->vsync_wait);
                 }
+
+                /* Set only when controller is disabled and at the end of
+                 * active frame
+                 */
+                if (stat & BIT(0)) {
+                        frame_done_flag = 1;
+                        wake_up_interruptible(&frame_done_wq);
+                }
         }
 
         lcdc_write(0, LCD_END_OF_INT_IND_REG);
@@ -789,7 +855,7 @@ static irqreturn_t lcdc_irq_handler_rev01(int irq, void *arg)
         u32 reg_ras;
 
         if ((stat & LCD_SYNC_LOST) && (stat & LCD_FIFO_UNDERFLOW)) {
-                lcd_disable_raster();
+                lcd_disable_raster(false);
                 lcdc_write(stat, LCD_STAT_REG);
                 lcd_enable_raster();
         } else if (stat & LCD_PL_LOAD_DONE) {
@@ -799,7 +865,7 @@ static irqreturn_t lcdc_irq_handler_rev01(int irq, void *arg)
                  * interrupt via the following write to the status register. If
                  * this is done after then one gets multiple PL done interrupts.
                  */
-                lcd_disable_raster();
+                lcd_disable_raster(false);
 
                 lcdc_write(stat, LCD_STAT_REG);
 
@@ -842,6 +908,9 @@ static int fb_check_var(struct fb_var_screeninfo *var,
 {
         int err = 0;
 
+        if (var->bits_per_pixel > 16 && lcd_revision == LCD_VERSION_1)
+                return -EINVAL;
+
         switch (var->bits_per_pixel) {
         case 1:
         case 8:
@@ -877,6 +946,26 @@ static int fb_check_var(struct fb_var_screeninfo *var,
                 var->transp.length = 0;
                 var->nonstd = 0;
                 break;
+        case 24:
+                var->red.offset = 16;
+                var->red.length = 8;
+                var->green.offset = 8;
+                var->green.length = 8;
+                var->blue.offset = 0;
+                var->blue.length = 8;
+                var->nonstd = 0;
+                break;
+        case 32:
+                var->transp.offset = 24;
+                var->transp.length = 8;
+                var->red.offset = 16;
+                var->red.length = 8;
+                var->green.offset = 8;
+                var->green.length = 8;
+                var->blue.offset = 0;
+                var->blue.length = 8;
+                var->nonstd = 0;
+                break;
         default:
                 err = -EINVAL;
         }
@@ -898,9 +987,10 @@ static int lcd_da8xx_cpufreq_transition(struct notifier_block *nb,
         if (val == CPUFREQ_POSTCHANGE) {
                 if (par->lcd_fck_rate != clk_get_rate(par->lcdc_clk)) {
                         par->lcd_fck_rate = clk_get_rate(par->lcdc_clk);
-                        lcd_disable_raster();
+                        lcd_disable_raster(true);
                         lcd_calc_clk_divider(par);
-                        lcd_enable_raster();
+                        if (par->blank == FB_BLANK_UNBLANK)
+                                lcd_enable_raster();
                 }
         }
 
@@ -935,7 +1025,7 @@ static int __devexit fb_remove(struct platform_device *dev)
                 if (par->panel_power_ctrl)
                         par->panel_power_ctrl(0);
 
-                lcd_disable_raster();
+                lcd_disable_raster(true);
                 lcdc_write(0, LCD_RASTER_CTRL_REG);
 
                 /* disable DMA  */
@@ -948,8 +1038,8 @@ static int __devexit fb_remove(struct platform_device *dev)
                 dma_free_coherent(NULL, par->vram_size, par->vram_virt,
                                   par->vram_phys);
                 free_irq(par->irq, par);
-                clk_disable(par->lcdc_clk);
-                clk_put(par->lcdc_clk);
+                pm_runtime_put_sync(&dev->dev);
+                pm_runtime_disable(&dev->dev);
                 framebuffer_release(info);
                 iounmap(da8xx_fb_reg_base);
                 release_mem_region(lcdc_regs->start, resource_size(lcdc_regs));
@@ -1051,7 +1141,7 @@ static int cfb_blank(int blank, struct fb_info *info)
                 if (par->panel_power_ctrl)
                         par->panel_power_ctrl(0);
 
-                lcd_disable_raster();
+                lcd_disable_raster(true);
                 break;
         default:
                 ret = -EINVAL;
@@ -1183,9 +1273,9 @@ static int __devinit fb_probe(struct platform_device *device)
                 ret = -ENODEV;
                 goto err_ioremap;
         }
-        ret = clk_enable(fb_clk);
-        if (ret)
-                goto err_clk_put;
+
+        pm_runtime_enable(&device->dev);
+        pm_runtime_get_sync(&device->dev);
 
         /* Determine LCD IP Version */
         switch (lcdc_read(LCD_PID_REG)) {
@@ -1213,7 +1303,7 @@ static int __devinit fb_probe(struct platform_device *device)
         if (i == ARRAY_SIZE(known_lcd_panels)) {
                 dev_err(&device->dev, "GLCD: No valid panel found\n");
                 ret = -ENODEV;
-                goto err_clk_disable;
+                goto err_pm_runtime_disable;
         } else
                 dev_info(&device->dev, "GLCD: Found %s panel\n",
                                         fb_pdata->type);
@@ -1225,7 +1315,7 @@ static int __devinit fb_probe(struct platform_device *device)
         if (!da8xx_fb_info) {
                 dev_dbg(&device->dev, "Memory allocation failed for fb_info\n");
                 ret = -ENOMEM;
-                goto err_clk_disable;
+                goto err_pm_runtime_disable;
         }
 
         par = da8xx_fb_info->par;
@@ -1356,8 +1446,10 @@ static int __devinit fb_probe(struct platform_device *device)
 
         if (lcd_revision == LCD_VERSION_1)
                 lcdc_irq_handler = lcdc_irq_handler_rev01;
-        else
+        else {
+                init_waitqueue_head(&frame_done_wq);
                 lcdc_irq_handler = lcdc_irq_handler_rev02;
+        }
 
         ret = request_irq(par->irq, lcdc_irq_handler, 0,
                         DRIVER_NAME, par);
@@ -1385,11 +1477,9 @@ err_release_fb_mem:
 err_release_fb:
         framebuffer_release(da8xx_fb_info);
 
-err_clk_disable:
-        clk_disable(fb_clk);
-
-err_clk_put:
-        clk_put(fb_clk);
+err_pm_runtime_disable:
+        pm_runtime_put_sync(&device->dev);
+        pm_runtime_disable(&device->dev);
 
 err_ioremap:
         iounmap(da8xx_fb_reg_base);
@@ -1401,6 +1491,69 @@ err_request_mem:
 }
 
 #ifdef CONFIG_PM
+struct lcdc_context {
+        u32 clk_enable;
+        u32 ctrl;
+        u32 dma_ctrl;
+        u32 raster_timing_0;
+        u32 raster_timing_1;
+        u32 raster_timing_2;
+        u32 int_enable_set;
+        u32 dma_frm_buf_base_addr_0;
+        u32 dma_frm_buf_ceiling_addr_0;
+        u32 dma_frm_buf_base_addr_1;
+        u32 dma_frm_buf_ceiling_addr_1;
+        u32 raster_ctrl;
+} reg_context;
+
+static void lcd_context_save(void)
+{
+        if (lcd_revision == LCD_VERSION_2) {
+                reg_context.clk_enable = lcdc_read(LCD_CLK_ENABLE_REG);
+                reg_context.int_enable_set = lcdc_read(LCD_INT_ENABLE_SET_REG);
+        }
+
+        reg_context.ctrl = lcdc_read(LCD_CTRL_REG);
+        reg_context.dma_ctrl = lcdc_read(LCD_DMA_CTRL_REG);
+        reg_context.raster_timing_0 = lcdc_read(LCD_RASTER_TIMING_0_REG);
+        reg_context.raster_timing_1 = lcdc_read(LCD_RASTER_TIMING_1_REG);
+        reg_context.raster_timing_2 = lcdc_read(LCD_RASTER_TIMING_2_REG);
+        reg_context.dma_frm_buf_base_addr_0 =
+                lcdc_read(LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
+        reg_context.dma_frm_buf_ceiling_addr_0 =
+                lcdc_read(LCD_DMA_FRM_BUF_CEILING_ADDR_0_REG);
+        reg_context.dma_frm_buf_base_addr_1 =
+                lcdc_read(LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
+        reg_context.dma_frm_buf_ceiling_addr_1 =
+                lcdc_read(LCD_DMA_FRM_BUF_CEILING_ADDR_1_REG);
+        reg_context.raster_ctrl = lcdc_read(LCD_RASTER_CTRL_REG);
+        return;
+}
+
+static void lcd_context_restore(void)
+{
+        if (lcd_revision == LCD_VERSION_2) {
+                lcdc_write(reg_context.clk_enable, LCD_CLK_ENABLE_REG);
+                lcdc_write(reg_context.int_enable_set, LCD_INT_ENABLE_SET_REG);
+        }
+
+        lcdc_write(reg_context.ctrl, LCD_CTRL_REG);
+        lcdc_write(reg_context.dma_ctrl, LCD_DMA_CTRL_REG);
+        lcdc_write(reg_context.raster_timing_0, LCD_RASTER_TIMING_0_REG);
+        lcdc_write(reg_context.raster_timing_1, LCD_RASTER_TIMING_1_REG);
+        lcdc_write(reg_context.raster_timing_2, LCD_RASTER_TIMING_2_REG);
+        lcdc_write(reg_context.dma_frm_buf_base_addr_0,
+                        LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
+        lcdc_write(reg_context.dma_frm_buf_ceiling_addr_0,
+                        LCD_DMA_FRM_BUF_CEILING_ADDR_0_REG);
+        lcdc_write(reg_context.dma_frm_buf_base_addr_1,
+                        LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
+        lcdc_write(reg_context.dma_frm_buf_ceiling_addr_1,
+                        LCD_DMA_FRM_BUF_CEILING_ADDR_1_REG);
+        lcdc_write(reg_context.raster_ctrl, LCD_RASTER_CTRL_REG);
+        return;
+}
+
 static int fb_suspend(struct platform_device *dev, pm_message_t state)
 {
         struct fb_info *info = platform_get_drvdata(dev);
@@ -1411,8 +1564,9 @@ static int fb_suspend(struct platform_device *dev, pm_message_t state)
                 par->panel_power_ctrl(0);
 
         fb_set_suspend(info, 1);
-        lcd_disable_raster();
-        clk_disable(par->lcdc_clk);
+        lcd_disable_raster(true);
+        lcd_context_save();
+        pm_runtime_put_sync(&dev->dev);
         console_unlock();
 
         return 0;
@@ -1423,11 +1577,14 @@ static int fb_resume(struct platform_device *dev)
         struct da8xx_fb_par *par = info->par;
 
         console_lock();
-        clk_enable(par->lcdc_clk);
-        lcd_enable_raster();
+        pm_runtime_get_sync(&dev->dev);
+        lcd_context_restore();
+        if (par->blank == FB_BLANK_UNBLANK) {
+                lcd_enable_raster();
 
-        if (par->panel_power_ctrl)
-                par->panel_power_ctrl(1);
+                if (par->panel_power_ctrl)
+                        par->panel_power_ctrl(1);
+        }
 
         fb_set_suspend(info, 0);
         console_unlock();
diff --git a/drivers/video/ep93xx-fb.c b/drivers/video/ep93xx-fb.c
index f2c092da84b0..755ef3e65caf 100644
--- a/drivers/video/ep93xx-fb.c
+++ b/drivers/video/ep93xx-fb.c
@@ -529,7 +529,8 @@ static int __devinit ep93xxfb_probe(struct platform_device *pdev)
          * any of the framebuffer registers.
          */
         fbi->res = res;
-        fbi->mmio_base = ioremap(res->start, resource_size(res));
+        fbi->mmio_base = devm_ioremap(&pdev->dev, res->start,
+                                      resource_size(res));
         if (!fbi->mmio_base) {
                 err = -ENXIO;
                 goto failed_resource;
@@ -553,20 +554,20 @@ static int __devinit ep93xxfb_probe(struct platform_device *pdev)
         if (err == 0) {
                 dev_err(info->dev, "No suitable video mode found\n");
                 err = -EINVAL;
-                goto failed_mode;
+                goto failed_resource;
         }
 
         if (mach_info->setup) {
                 err = mach_info->setup(pdev);
                 if (err)
-                        goto failed_mode;
+                        goto failed_resource;
         }
 
         err = ep93xxfb_check_var(&info->var, info);
         if (err)
                 goto failed_check;
 
-        fbi->clk = clk_get(info->dev, NULL);
+        fbi->clk = devm_clk_get(&pdev->dev, NULL);
         if (IS_ERR(fbi->clk)) {
                 err = PTR_ERR(fbi->clk);
                 fbi->clk = NULL;
@@ -578,19 +579,15 @@ static int __devinit ep93xxfb_probe(struct platform_device *pdev)
 
         err = register_framebuffer(info);
         if (err)
-                goto failed;
+                goto failed_check;
 
         dev_info(info->dev, "registered. Mode = %dx%d-%d\n",
                  info->var.xres, info->var.yres, info->var.bits_per_pixel);
         return 0;
 
-failed:
-        clk_put(fbi->clk);
 failed_check:
         if (fbi->mach_info->teardown)
                 fbi->mach_info->teardown(pdev);
-failed_mode:
-        iounmap(fbi->mmio_base);
 failed_resource:
         ep93xxfb_dealloc_videomem(info);
 failed_videomem:
@@ -609,8 +606,6 @@ static int __devexit ep93xxfb_remove(struct platform_device *pdev)
 
         unregister_framebuffer(info);
         clk_disable(fbi->clk);
-        clk_put(fbi->clk);
-        iounmap(fbi->mmio_base);
         ep93xxfb_dealloc_videomem(info);
         fb_dealloc_cmap(&info->cmap);
 
diff --git a/drivers/video/exynos/exynos_dp_core.c b/drivers/video/exynos/exynos_dp_core.c
index c6c016a506ce..d55470e75412 100644
--- a/drivers/video/exynos/exynos_dp_core.c
+++ b/drivers/video/exynos/exynos_dp_core.c
@@ -29,6 +29,9 @@ static int exynos_dp_init_dp(struct exynos_dp_device *dp)
 
         exynos_dp_swreset(dp);
 
+        exynos_dp_init_analog_param(dp);
+        exynos_dp_init_interrupt(dp);
+
         /* SW defined function Normal operation */
         exynos_dp_enable_sw_function(dp);
 
@@ -260,7 +263,7 @@ static void exynos_dp_set_lane_lane_pre_emphasis(struct exynos_dp_device *dp,
 
 static void exynos_dp_link_start(struct exynos_dp_device *dp)
 {
-        u8 buf[5];
+        u8 buf[4];
         int lane;
         int lane_count;
 
@@ -295,10 +298,10 @@ static void exynos_dp_link_start(struct exynos_dp_device *dp)
         exynos_dp_set_training_pattern(dp, TRAINING_PTN1);
 
         /* Set RX training pattern */
-        buf[0] = DPCD_SCRAMBLING_DISABLED |
-                 DPCD_TRAINING_PATTERN_1;
         exynos_dp_write_byte_to_dpcd(dp,
-                DPCD_ADDR_TRAINING_PATTERN_SET, buf[0]);
+                DPCD_ADDR_TRAINING_PATTERN_SET,
+                DPCD_SCRAMBLING_DISABLED |
+                DPCD_TRAINING_PATTERN_1);
 
         for (lane = 0; lane < lane_count; lane++)
                 buf[lane] = DPCD_PRE_EMPHASIS_PATTERN2_LEVEL0 |
@@ -308,7 +311,7 @@ static void exynos_dp_link_start(struct exynos_dp_device *dp)
                 lane_count, buf);
 }
 
-static unsigned char exynos_dp_get_lane_status(u8 link_status[6], int lane)
+static unsigned char exynos_dp_get_lane_status(u8 link_status[2], int lane)
 {
         int shift = (lane & 1) * 4;
         u8 link_value = link_status[lane>>1];
@@ -316,7 +319,7 @@ static unsigned char exynos_dp_get_lane_status(u8 link_status[6], int lane)
         return (link_value >> shift) & 0xf;
 }
 
-static int exynos_dp_clock_recovery_ok(u8 link_status[6], int lane_count)
+static int exynos_dp_clock_recovery_ok(u8 link_status[2], int lane_count)
 {
         int lane;
         u8 lane_status;
@@ -329,22 +332,23 @@ static int exynos_dp_clock_recovery_ok(u8 link_status[6], int lane_count)
         return 0;
 }
 
-static int exynos_dp_channel_eq_ok(u8 link_status[6], int lane_count)
+static int exynos_dp_channel_eq_ok(u8 link_align[3], int lane_count)
 {
         int lane;
         u8 lane_align;
         u8 lane_status;
 
-        lane_align = link_status[2];
+        lane_align = link_align[2];
         if ((lane_align & DPCD_INTERLANE_ALIGN_DONE) == 0)
                 return -EINVAL;
 
         for (lane = 0; lane < lane_count; lane++) {
-                lane_status = exynos_dp_get_lane_status(link_status, lane);
+                lane_status = exynos_dp_get_lane_status(link_align, lane);
                 lane_status &= DPCD_CHANNEL_EQ_BITS;
                 if (lane_status != DPCD_CHANNEL_EQ_BITS)
                         return -EINVAL;
         }
+
         return 0;
 }
 
@@ -417,69 +421,17 @@ static unsigned int exynos_dp_get_lane_link_training(
 
 static void exynos_dp_reduce_link_rate(struct exynos_dp_device *dp)
 {
-        if (dp->link_train.link_rate == LINK_RATE_2_70GBPS) {
-                /* set to reduced bit rate */
-                dp->link_train.link_rate = LINK_RATE_1_62GBPS;
-                dev_err(dp->dev, "set to bandwidth %.2x\n",
-                        dp->link_train.link_rate);
-                dp->link_train.lt_state = START;
-        } else {
-                exynos_dp_training_pattern_dis(dp);
-                /* set enhanced mode if available */
-                exynos_dp_set_enhanced_mode(dp);
-                dp->link_train.lt_state = FAILED;
-        }
-}
+        exynos_dp_training_pattern_dis(dp);
+        exynos_dp_set_enhanced_mode(dp);
 
-static void exynos_dp_get_adjust_train(struct exynos_dp_device *dp,
-                                u8 adjust_request[2])
-{
-        int lane;
-        int lane_count;
-        u8 voltage_swing;
-        u8 pre_emphasis;
-        u8 training_lane;
-
-        lane_count = dp->link_train.lane_count;
-        for (lane = 0; lane < lane_count; lane++) {
-                voltage_swing = exynos_dp_get_adjust_request_voltage(
-                                                adjust_request, lane);
-                pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
-                                                adjust_request, lane);
-                training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
-                                DPCD_PRE_EMPHASIS_SET(pre_emphasis);
-
-                if (voltage_swing == VOLTAGE_LEVEL_3 ||
-                   pre_emphasis == PRE_EMPHASIS_LEVEL_3) {
-                        training_lane |= DPCD_MAX_SWING_REACHED;
-                        training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
-                }
-                dp->link_train.training_lane[lane] = training_lane;
-        }
-}
-
-static int exynos_dp_check_max_cr_loop(struct exynos_dp_device *dp,
-                                        u8 voltage_swing)
-{
-        int lane;
-        int lane_count;
-
-        lane_count = dp->link_train.lane_count;
-        for (lane = 0; lane < lane_count; lane++) {
-                if (voltage_swing == VOLTAGE_LEVEL_3 ||
-                        dp->link_train.cr_loop[lane] == MAX_CR_LOOP)
-                        return -EINVAL;
-        }
-        return 0;
+        dp->link_train.lt_state = FAILED;
 }
 
 static int exynos_dp_process_clock_recovery(struct exynos_dp_device *dp)
 {
-        u8 data;
-        u8 link_status[6];
+        u8 link_status[2];
         int lane;
         int lane_count;
-        u8 buf[5];
 
         u8 adjust_request[2];
         u8 voltage_swing;
@@ -488,100 +440,154 @@ static int exynos_dp_process_clock_recovery(struct exynos_dp_device *dp)
 
         usleep_range(100, 101);
 
-        exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
-                                6, link_status);
         lane_count = dp->link_train.lane_count;
 
+        exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
+                                2, link_status);
+
         if (exynos_dp_clock_recovery_ok(link_status, lane_count) == 0) {
                 /* set training pattern 2 for EQ */
                 exynos_dp_set_training_pattern(dp, TRAINING_PTN2);
 
-                adjust_request[0] = link_status[4];
-                adjust_request[1] = link_status[5];
+                for (lane = 0; lane < lane_count; lane++) {
+                        exynos_dp_read_bytes_from_dpcd(dp,
+                                        DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
+                                        2, adjust_request);
+                        voltage_swing = exynos_dp_get_adjust_request_voltage(
+                                                        adjust_request, lane);
+                        pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
+                                                        adjust_request, lane);
+                        training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
+                                        DPCD_PRE_EMPHASIS_SET(pre_emphasis);
 
-                exynos_dp_get_adjust_train(dp, adjust_request);
+                        if (voltage_swing == VOLTAGE_LEVEL_3)
+                                training_lane |= DPCD_MAX_SWING_REACHED;
+                        if (pre_emphasis == PRE_EMPHASIS_LEVEL_3)
+                                training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
 
-                buf[0] = DPCD_SCRAMBLING_DISABLED |
-                         DPCD_TRAINING_PATTERN_2;
-                exynos_dp_write_byte_to_dpcd(dp,
-                        DPCD_ADDR_TRAINING_PATTERN_SET,
-                        buf[0]);
+                        dp->link_train.training_lane[lane] = training_lane;
 
-                for (lane = 0; lane < lane_count; lane++) {
                         exynos_dp_set_lane_link_training(dp,
                                 dp->link_train.training_lane[lane],
                                 lane);
-                        buf[lane] = dp->link_train.training_lane[lane];
-                        exynos_dp_write_byte_to_dpcd(dp,
-                                DPCD_ADDR_TRAINING_LANE0_SET + lane,
-                                buf[lane]);
                 }
-                dp->link_train.lt_state = EQUALIZER_TRAINING;
-        } else {
-                exynos_dp_read_byte_from_dpcd(dp,
-                        DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
-                        &data);
-                adjust_request[0] = data;
 
-                exynos_dp_read_byte_from_dpcd(dp,
-                        DPCD_ADDR_ADJUST_REQUEST_LANE2_3,
-                        &data);
-                adjust_request[1] = data;
+                exynos_dp_write_byte_to_dpcd(dp,
+                        DPCD_ADDR_TRAINING_PATTERN_SET,
+                        DPCD_SCRAMBLING_DISABLED |
+                        DPCD_TRAINING_PATTERN_2);
+
+                exynos_dp_write_bytes_to_dpcd(dp,
+                        DPCD_ADDR_TRAINING_LANE0_SET,
+                        lane_count,
+                        dp->link_train.training_lane);
 
+                dev_info(dp->dev, "Link Training Clock Recovery success\n");
+                dp->link_train.lt_state = EQUALIZER_TRAINING;
+        } else {
                 for (lane = 0; lane < lane_count; lane++) {
                         training_lane = exynos_dp_get_lane_link_training(
                                                         dp, lane);
+                        exynos_dp_read_bytes_from_dpcd(dp,
+                                        DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
+                                        2, adjust_request);
                         voltage_swing = exynos_dp_get_adjust_request_voltage(
                                                         adjust_request, lane);
                         pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
                                                         adjust_request, lane);
-                        if ((DPCD_VOLTAGE_SWING_GET(training_lane) == voltage_swing) &&
-                            (DPCD_PRE_EMPHASIS_GET(training_lane) == pre_emphasis))
-                                dp->link_train.cr_loop[lane]++;
-                        dp->link_train.training_lane[lane] = training_lane;
-                }
 
-                if (exynos_dp_check_max_cr_loop(dp, voltage_swing) != 0) {
-                        exynos_dp_reduce_link_rate(dp);
-                } else {
-                        exynos_dp_get_adjust_train(dp, adjust_request);
+                        if (voltage_swing == VOLTAGE_LEVEL_3 ||
+                            pre_emphasis == PRE_EMPHASIS_LEVEL_3) {
+                                dev_err(dp->dev, "voltage or pre emphasis reached max level\n");
+                                goto reduce_link_rate;
+                        }
 
-                        for (lane = 0; lane < lane_count; lane++) {
-                                exynos_dp_set_lane_link_training(dp,
-                                        dp->link_train.training_lane[lane],
-                                        lane);
-                                buf[lane] = dp->link_train.training_lane[lane];
-                                exynos_dp_write_byte_to_dpcd(dp,
-                                        DPCD_ADDR_TRAINING_LANE0_SET + lane,
-                                        buf[lane]);
+                        if ((DPCD_VOLTAGE_SWING_GET(training_lane) ==
+                                        voltage_swing) &&
+                           (DPCD_PRE_EMPHASIS_GET(training_lane) ==
+                                        pre_emphasis)) {
+                                dp->link_train.cr_loop[lane]++;
+                                if (dp->link_train.cr_loop[lane] == MAX_CR_LOOP) {
+                                        dev_err(dp->dev, "CR Max loop\n");
+                                        goto reduce_link_rate;
+                                }
                         }
+
+                        training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
+                                        DPCD_PRE_EMPHASIS_SET(pre_emphasis);
+
+                        if (voltage_swing == VOLTAGE_LEVEL_3)
+                                training_lane |= DPCD_MAX_SWING_REACHED;
+                        if (pre_emphasis == PRE_EMPHASIS_LEVEL_3)
+                                training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
+
+                        dp->link_train.training_lane[lane] = training_lane;
+
+                        exynos_dp_set_lane_link_training(dp,
+                                dp->link_train.training_lane[lane], lane);
                 }
+
+                exynos_dp_write_bytes_to_dpcd(dp,
+                        DPCD_ADDR_TRAINING_LANE0_SET,
+                        lane_count,
+                        dp->link_train.training_lane);
         }
 
         return 0;
+
+reduce_link_rate:
+        exynos_dp_reduce_link_rate(dp);
+        return -EIO;
 }
 
 static int exynos_dp_process_equalizer_training(struct exynos_dp_device *dp)
 {
-        u8 link_status[6];
+        u8 link_status[2];
+        u8 link_align[3];
         int lane;
         int lane_count;
-        u8 buf[5];
         u32 reg;
 
         u8 adjust_request[2];
+        u8 voltage_swing;
+        u8 pre_emphasis;
+        u8 training_lane;
 
         usleep_range(400, 401);
 
-        exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
-                                6, link_status);
         lane_count = dp->link_train.lane_count;
 
+        exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
+                                2, link_status);
+
         if (exynos_dp_clock_recovery_ok(link_status, lane_count) == 0) {
-                adjust_request[0] = link_status[4];
-                adjust_request[1] = link_status[5];
+                link_align[0] = link_status[0];
+                link_align[1] = link_status[1];
 
-                if (exynos_dp_channel_eq_ok(link_status, lane_count) == 0) {
+                exynos_dp_read_byte_from_dpcd(dp,
+                        DPCD_ADDR_LANE_ALIGN_STATUS_UPDATED,
+                        &link_align[2]);
+
+                for (lane = 0; lane < lane_count; lane++) {
+                        exynos_dp_read_bytes_from_dpcd(dp,
+                                        DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
+                                        2, adjust_request);
+                        voltage_swing = exynos_dp_get_adjust_request_voltage(
+                                                        adjust_request, lane);
+                        pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
+                                                        adjust_request, lane);
+                        training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
+                                        DPCD_PRE_EMPHASIS_SET(pre_emphasis);
+
+                        if (voltage_swing == VOLTAGE_LEVEL_3)
+                                training_lane |= DPCD_MAX_SWING_REACHED;
+                        if (pre_emphasis == PRE_EMPHASIS_LEVEL_3)
+                                training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
+
+                        dp->link_train.training_lane[lane] = training_lane;
+                }
+
+                if (exynos_dp_channel_eq_ok(link_align, lane_count) == 0) {
                         /* traing pattern Set to Normal */
                         exynos_dp_training_pattern_dis(dp);
 
@@ -596,39 +602,42 @@ static int exynos_dp_process_equalizer_training(struct exynos_dp_device *dp)
                         dp->link_train.lane_count = reg;
                         dev_dbg(dp->dev, "final lane count = %.2x\n",
                                 dp->link_train.lane_count);
+
                         /* set enhanced mode if available */
                         exynos_dp_set_enhanced_mode(dp);
-
                         dp->link_train.lt_state = FINISHED;
                 } else {
                         /* not all locked */
                         dp->link_train.eq_loop++;
 
                         if (dp->link_train.eq_loop > MAX_EQ_LOOP) {
-                                exynos_dp_reduce_link_rate(dp);
-                        } else {
-                                exynos_dp_get_adjust_train(dp, adjust_request);
-
-                                for (lane = 0; lane < lane_count; lane++) {
-                                        exynos_dp_set_lane_link_training(dp,
-                                                dp->link_train.training_lane[lane],
-                                                lane);
-                                        buf[lane] = dp->link_train.training_lane[lane];
-                                        exynos_dp_write_byte_to_dpcd(dp,
-                                                DPCD_ADDR_TRAINING_LANE0_SET + lane,
-                                                buf[lane]);
-                                }
+                                dev_err(dp->dev, "EQ Max loop\n");
+                                goto reduce_link_rate;
                         }
+
+                        for (lane = 0; lane < lane_count; lane++)
+                                exynos_dp_set_lane_link_training(dp,
+                                        dp->link_train.training_lane[lane],
+                                        lane);
+
+                        exynos_dp_write_bytes_to_dpcd(dp,
+                                DPCD_ADDR_TRAINING_LANE0_SET,
+                                lane_count,
+                                dp->link_train.training_lane);
                 }
         } else {
-                exynos_dp_reduce_link_rate(dp);
+                goto reduce_link_rate;
         }
 
         return 0;
+
+reduce_link_rate:
+        exynos_dp_reduce_link_rate(dp);
+        return -EIO;
 }
 
 static void exynos_dp_get_max_rx_bandwidth(struct exynos_dp_device *dp,
-                        u8 *bandwidth)
+                                        u8 *bandwidth)
 {
         u8 data;
 
@@ -641,7 +650,7 @@ static void exynos_dp_get_max_rx_bandwidth(struct exynos_dp_device *dp,
 }
 
 static void exynos_dp_get_max_rx_lane_count(struct exynos_dp_device *dp,
-                        u8 *lane_count)
+                                        u8 *lane_count)
 {
         u8 data;
 
@@ -693,13 +702,7 @@ static void exynos_dp_init_training(struct exynos_dp_device *dp,
 static int exynos_dp_sw_link_training(struct exynos_dp_device *dp)
 {
         int retval = 0;
-        int training_finished;
-
-        /* Turn off unnecessary lane */
-        if (dp->link_train.lane_count == 1)
-                exynos_dp_set_analog_power_down(dp, CH1_BLOCK, 1);
-
-        training_finished = 0;
+        int training_finished = 0;
 
         dp->link_train.lt_state = START;
 
@@ -710,10 +713,14 @@ static int exynos_dp_sw_link_training(struct exynos_dp_device *dp)
                         exynos_dp_link_start(dp);
                         break;
                 case CLOCK_RECOVERY:
-                        exynos_dp_process_clock_recovery(dp);
+                        retval = exynos_dp_process_clock_recovery(dp);
+                        if (retval)
+                                dev_err(dp->dev, "LT CR failed!\n");
                         break;
                 case EQUALIZER_TRAINING:
-                        exynos_dp_process_equalizer_training(dp);
+                        retval = exynos_dp_process_equalizer_training(dp);
+                        if (retval)
+                                dev_err(dp->dev, "LT EQ failed!\n");
                         break;
                 case FINISHED:
                         training_finished = 1;
@@ -872,40 +879,33 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
 
         dp->dev = &pdev->dev;
 
-        dp->clock = clk_get(&pdev->dev, "dp");
+        dp->clock = devm_clk_get(&pdev->dev, "dp");
         if (IS_ERR(dp->clock)) {
                 dev_err(&pdev->dev, "failed to get clock\n");
                 return PTR_ERR(dp->clock);
         }
 
-        clk_enable(dp->clock);
+        clk_prepare_enable(dp->clock);
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!res) {
-                dev_err(&pdev->dev, "failed to get registers\n");
-                ret = -EINVAL;
-                goto err_clock;
-        }
 
         dp->reg_base = devm_request_and_ioremap(&pdev->dev, res);
         if (!dp->reg_base) {
                 dev_err(&pdev->dev, "failed to ioremap\n");
-                ret = -ENOMEM;
-                goto err_clock;
+                return -ENOMEM;
         }
 
         dp->irq = platform_get_irq(pdev, 0);
         if (!dp->irq) {
                 dev_err(&pdev->dev, "failed to get irq\n");
-                ret = -ENODEV;
-                goto err_clock;
+                return -ENODEV;
         }
 
         ret = devm_request_irq(&pdev->dev, dp->irq, exynos_dp_irq_handler, 0,
                                 "exynos-dp", dp);
         if (ret) {
                 dev_err(&pdev->dev, "failed to request irq\n");
-                goto err_clock;
+                return ret;
         }
 
         dp->video_info = pdata->video_info;
@@ -917,7 +917,7 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
         ret = exynos_dp_detect_hpd(dp);
         if (ret) {
                 dev_err(&pdev->dev, "unable to detect hpd\n");
-                goto err_clock;
+                return ret;
         }
 
         exynos_dp_handle_edid(dp);
@@ -926,7 +926,7 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
                                 dp->video_info->link_rate);
         if (ret) {
                 dev_err(&pdev->dev, "unable to do link train\n");
-                goto err_clock;
+                return ret;
         }
 
         exynos_dp_enable_scramble(dp, 1);
@@ -940,17 +940,12 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
         ret = exynos_dp_config_video(dp, dp->video_info);
         if (ret) {
                 dev_err(&pdev->dev, "unable to config video\n");
-                goto err_clock;
+                return ret;
         }
 
         platform_set_drvdata(pdev, dp);
 
         return 0;
-
-err_clock:
-        clk_put(dp->clock);
-
-        return ret;
 }
 
 static int __devexit exynos_dp_remove(struct platform_device *pdev)
@@ -961,8 +956,7 @@ static int __devexit exynos_dp_remove(struct platform_device *pdev)
         if (pdata && pdata->phy_exit)
                 pdata->phy_exit();
 
-        clk_disable(dp->clock);
-        clk_put(dp->clock);
+        clk_disable_unprepare(dp->clock);
 
         return 0;
 }
@@ -977,7 +971,7 @@ static int exynos_dp_suspend(struct device *dev)
         if (pdata && pdata->phy_exit)
                 pdata->phy_exit();
 
-        clk_disable(dp->clock);
+        clk_disable_unprepare(dp->clock);
 
         return 0;
 }
@@ -991,7 +985,7 @@ static int exynos_dp_resume(struct device *dev)
         if (pdata && pdata->phy_init)
                 pdata->phy_init();
 
-        clk_enable(dp->clock);
+        clk_prepare_enable(dp->clock);
 
         exynos_dp_init_dp(dp);
 
diff --git a/drivers/video/exynos/exynos_dp_core.h b/drivers/video/exynos/exynos_dp_core.h
index 8526e548c385..57b8a6531c0e 100644
--- a/drivers/video/exynos/exynos_dp_core.h
+++ b/drivers/video/exynos/exynos_dp_core.h
@@ -43,7 +43,7 @@ void exynos_dp_init_interrupt(struct exynos_dp_device *dp);
 void exynos_dp_reset(struct exynos_dp_device *dp);
 void exynos_dp_swreset(struct exynos_dp_device *dp);
 void exynos_dp_config_interrupt(struct exynos_dp_device *dp);
-u32 exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp);
+enum pll_status exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp);
 void exynos_dp_set_pll_power_down(struct exynos_dp_device *dp, bool enable);
 void exynos_dp_set_analog_power_down(struct exynos_dp_device *dp,
                                 enum analog_power_block block,
@@ -105,7 +105,7 @@ u32 exynos_dp_get_lane1_link_training(struct exynos_dp_device *dp);
 u32 exynos_dp_get_lane2_link_training(struct exynos_dp_device *dp);
 u32 exynos_dp_get_lane3_link_training(struct exynos_dp_device *dp);
 void exynos_dp_reset_macro(struct exynos_dp_device *dp);
-int exynos_dp_init_video(struct exynos_dp_device *dp);
+void exynos_dp_init_video(struct exynos_dp_device *dp);
 
 void exynos_dp_set_video_color_format(struct exynos_dp_device *dp,
                                 u32 color_depth,
@@ -144,7 +144,7 @@ void exynos_dp_disable_scrambling(struct exynos_dp_device *dp);
 #define DPCD_ADDR_TRAINING_PATTERN_SET          0x0102
 #define DPCD_ADDR_TRAINING_LANE0_SET            0x0103
 #define DPCD_ADDR_LANE0_1_STATUS                0x0202
-#define DPCD_ADDR_LANE_ALIGN__STATUS_UPDATED    0x0204
+#define DPCD_ADDR_LANE_ALIGN_STATUS_UPDATED     0x0204
 #define DPCD_ADDR_ADJUST_REQUEST_LANE0_1        0x0206
 #define DPCD_ADDR_ADJUST_REQUEST_LANE2_3        0x0207
 #define DPCD_ADDR_TEST_REQUEST                  0x0218
diff --git a/drivers/video/exynos/exynos_dp_reg.c b/drivers/video/exynos/exynos_dp_reg.c
index 2db5b9aa250a..3f5ca8a0d5ea 100644
--- a/drivers/video/exynos/exynos_dp_reg.c
+++ b/drivers/video/exynos/exynos_dp_reg.c
@@ -77,7 +77,7 @@ void exynos_dp_init_analog_param(struct exynos_dp_device *dp)
         writel(reg, dp->reg_base + EXYNOS_DP_ANALOG_CTL_3);
 
         reg = PD_RING_OSC | AUX_TERMINAL_CTRL_50_OHM |
-                TX_CUR1_2X | TX_CUR_8_MA;
+                TX_CUR1_2X | TX_CUR_16_MA;
         writel(reg, dp->reg_base + EXYNOS_DP_PLL_FILTER_CTL_1);
 
         reg = CH3_AMP_400_MV | CH2_AMP_400_MV |
@@ -148,9 +148,6 @@ void exynos_dp_reset(struct exynos_dp_device *dp)
         writel(0x2, dp->reg_base + EXYNOS_DP_M_AUD_GEN_FILTER_TH);
 
         writel(0x00000101, dp->reg_base + EXYNOS_DP_SOC_GENERAL_CTL);
-
-        exynos_dp_init_analog_param(dp);
-        exynos_dp_init_interrupt(dp);
 }
 
 void exynos_dp_swreset(struct exynos_dp_device *dp)
@@ -179,7 +176,7 @@ void exynos_dp_config_interrupt(struct exynos_dp_device *dp)
         writel(reg, dp->reg_base + EXYNOS_DP_INT_STA_MASK);
 }
 
-u32 exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp)
+enum pll_status exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp)
 {
         u32 reg;
 
@@ -401,6 +398,7 @@ int exynos_dp_start_aux_transaction(struct exynos_dp_device *dp)
 {
         int reg;
         int retval = 0;
+        int timeout_loop = 0;
 
         /* Enable AUX CH operation */
         reg = readl(dp->reg_base + EXYNOS_DP_AUX_CH_CTL_2);
@@ -409,8 +407,15 @@ int exynos_dp_start_aux_transaction(struct exynos_dp_device *dp)
 
         /* Is AUX CH command reply received? */
         reg = readl(dp->reg_base + EXYNOS_DP_INT_STA);
-        while (!(reg & RPLY_RECEIV))
+        while (!(reg & RPLY_RECEIV)) {
+                timeout_loop++;
+                if (DP_TIMEOUT_LOOP_COUNT < timeout_loop) {
+                        dev_err(dp->dev, "AUX CH command reply failed!\n");
+                        return -ETIMEDOUT;
+                }
                 reg = readl(dp->reg_base + EXYNOS_DP_INT_STA);
+                usleep_range(10, 11);
+        }
 
         /* Clear interrupt source for AUX CH command reply */
         writel(RPLY_RECEIV, dp->reg_base + EXYNOS_DP_INT_STA);
@@ -471,7 +476,8 @@ int exynos_dp_write_byte_to_dpcd(struct exynos_dp_device *dp,
                 if (retval == 0)
                         break;
                 else
-                        dev_err(dp->dev, "Aux Transaction fail!\n");
+                        dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+                                __func__);
         }
 
         return retval;
@@ -511,7 +517,8 @@ int exynos_dp_read_byte_from_dpcd(struct exynos_dp_device *dp,
                 if (retval == 0)
                         break;
                 else
-                        dev_err(dp->dev, "Aux Transaction fail!\n");
+                        dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+                                __func__);
         }
 
         /* Read data buffer */
@@ -575,7 +582,8 @@ int exynos_dp_write_bytes_to_dpcd(struct exynos_dp_device *dp,
                         if (retval == 0)
                                 break;
                         else
-                                dev_err(dp->dev, "Aux Transaction fail!\n");
+                                dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+                                        __func__);
                 }
 
                 start_offset += cur_data_count;
@@ -632,7 +640,8 @@ int exynos_dp_read_bytes_from_dpcd(struct exynos_dp_device *dp,
                         if (retval == 0)
                                 break;
                         else
-                                dev_err(dp->dev, "Aux Transaction fail!\n");
+                                dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+                                        __func__);
                 }
 
                 for (cur_data_idx = 0; cur_data_idx < cur_data_count;
@@ -677,7 +686,7 @@ int exynos_dp_select_i2c_device(struct exynos_dp_device *dp,
         /* Start AUX transaction */
         retval = exynos_dp_start_aux_transaction(dp);
         if (retval != 0)
-                dev_err(dp->dev, "Aux Transaction fail!\n");
+                dev_dbg(dp->dev, "%s: Aux Transaction fail!\n", __func__);
 
         return retval;
 }
@@ -717,7 +726,8 @@ int exynos_dp_read_byte_from_i2c(struct exynos_dp_device *dp,
                 if (retval == 0)
                         break;
                 else
-                        dev_err(dp->dev, "Aux Transaction fail!\n");
+                        dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+                                __func__);
         }
 
         /* Read data */
@@ -777,7 +787,9 @@ int exynos_dp_read_bytes_from_i2c(struct exynos_dp_device *dp,
                                 if (retval == 0)
                                         break;
                                 else
-                                        dev_err(dp->dev, "Aux Transaction fail!\n");
+                                        dev_dbg(dp->dev,
+                                                "%s: Aux Transaction fail!\n",
+                                                __func__);
                         }
                         /* Check if Rx sends defer */
                         reg = readl(dp->reg_base + EXYNOS_DP_AUX_RX_COMM);
@@ -883,7 +895,9 @@ void exynos_dp_set_lane0_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
         u32 reg;
 
-        reg = level << PRE_EMPHASIS_SET_SHIFT;
+        reg = readl(dp->reg_base + EXYNOS_DP_LN0_LINK_TRAINING_CTL);
+        reg &= ~PRE_EMPHASIS_SET_MASK;
+        reg |= level << PRE_EMPHASIS_SET_SHIFT;
         writel(reg, dp->reg_base + EXYNOS_DP_LN0_LINK_TRAINING_CTL);
 }
 
@@ -891,7 +905,9 @@ void exynos_dp_set_lane1_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
         u32 reg;
 
-        reg = level << PRE_EMPHASIS_SET_SHIFT;
+        reg = readl(dp->reg_base + EXYNOS_DP_LN1_LINK_TRAINING_CTL);
+        reg &= ~PRE_EMPHASIS_SET_MASK;
+        reg |= level << PRE_EMPHASIS_SET_SHIFT;
         writel(reg, dp->reg_base + EXYNOS_DP_LN1_LINK_TRAINING_CTL);
 }
 
@@ -899,7 +915,9 @@ void exynos_dp_set_lane2_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
         u32 reg;
 
-        reg = level << PRE_EMPHASIS_SET_SHIFT;
+        reg = readl(dp->reg_base + EXYNOS_DP_LN2_LINK_TRAINING_CTL);
+        reg &= ~PRE_EMPHASIS_SET_MASK;
+        reg |= level << PRE_EMPHASIS_SET_SHIFT;
         writel(reg, dp->reg_base + EXYNOS_DP_LN2_LINK_TRAINING_CTL);
 }
 
@@ -907,7 +925,9 @@ void exynos_dp_set_lane3_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
         u32 reg;
 
-        reg = level << PRE_EMPHASIS_SET_SHIFT;
+        reg = readl(dp->reg_base + EXYNOS_DP_LN3_LINK_TRAINING_CTL);
+        reg &= ~PRE_EMPHASIS_SET_MASK;
+        reg |= level << PRE_EMPHASIS_SET_SHIFT;
         writel(reg, dp->reg_base + EXYNOS_DP_LN3_LINK_TRAINING_CTL);
 }
 
@@ -994,7 +1014,7 @@ void exynos_dp_reset_macro(struct exynos_dp_device *dp)
         writel(reg, dp->reg_base + EXYNOS_DP_PHY_TEST);
 }
 
-int exynos_dp_init_video(struct exynos_dp_device *dp)
+void exynos_dp_init_video(struct exynos_dp_device *dp)
 {
         u32 reg;
 
@@ -1012,8 +1032,6 @@ int exynos_dp_init_video(struct exynos_dp_device *dp)
 
         reg = VID_HRES_TH(2) | VID_VRES_TH(0);
         writel(reg, dp->reg_base + EXYNOS_DP_VIDEO_CTL_8);
-
-        return 0;
 }
 
 void exynos_dp_set_video_color_format(struct exynos_dp_device *dp,
diff --git a/drivers/video/exynos/exynos_dp_reg.h b/drivers/video/exynos/exynos_dp_reg.h
index 125b27cd57ae..1f2f014cfe88 100644
--- a/drivers/video/exynos/exynos_dp_reg.h
+++ b/drivers/video/exynos/exynos_dp_reg.h
@@ -187,7 +187,7 @@
 #define PD_RING_OSC                             (0x1 << 6)
 #define AUX_TERMINAL_CTRL_50_OHM                (0x2 << 4)
 #define TX_CUR1_2X                              (0x1 << 2)
-#define TX_CUR_8_MA                             (0x2 << 0)
+#define TX_CUR_16_MA                            (0x3 << 0)
 
 /* EXYNOS_DP_TX_AMP_TUNING_CTL */
 #define CH3_AMP_400_MV                          (0x0 << 24)
@@ -285,6 +285,7 @@
 #define SW_TRAINING_PATTERN_SET_NORMAL          (0x0 << 0)
 
 /* EXYNOS_DP_LN0_LINK_TRAINING_CTL */
+#define PRE_EMPHASIS_SET_MASK                   (0x3 << 3)
 #define PRE_EMPHASIS_SET_SHIFT                  (3)
 
 /* EXYNOS_DP_DEBUG_CTL */
diff --git a/drivers/video/exynos/exynos_mipi_dsi.c b/drivers/video/exynos/exynos_mipi_dsi.c
index 663c308d0e73..07d70a3a628b 100644
--- a/drivers/video/exynos/exynos_mipi_dsi.c
+++ b/drivers/video/exynos/exynos_mipi_dsi.c
@@ -205,7 +205,8 @@ int exynos_mipi_dsi_register_lcd_device(struct mipi_dsim_lcd_device *lcd_dev)
         return 0;
 }
 
-struct mipi_dsim_ddi *exynos_mipi_dsi_find_lcd_device(struct mipi_dsim_lcd_driver *lcd_drv)
+static struct mipi_dsim_ddi *exynos_mipi_dsi_find_lcd_device(
+                                        struct mipi_dsim_lcd_driver *lcd_drv)
 {
         struct mipi_dsim_ddi *dsim_ddi, *next;
         struct mipi_dsim_lcd_device *lcd_dev;
@@ -265,7 +266,8 @@ int exynos_mipi_dsi_register_lcd_driver(struct mipi_dsim_lcd_driver *lcd_drv)
 
 }
 
-struct mipi_dsim_ddi *exynos_mipi_dsi_bind_lcd_ddi(struct mipi_dsim_device *dsim,
+static struct mipi_dsim_ddi *exynos_mipi_dsi_bind_lcd_ddi(
+                                                struct mipi_dsim_device *dsim,
                                                 const char *name)
 {
         struct mipi_dsim_ddi *dsim_ddi, *next;
@@ -373,6 +375,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
         dsim->clock = clk_get(&pdev->dev, "dsim0");
         if (IS_ERR(dsim->clock)) {
                 dev_err(&pdev->dev, "failed to get dsim clock source\n");
+                ret = -ENODEV;
                 goto err_clock_get;
         }
 
@@ -381,6 +384,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (!res) {
                 dev_err(&pdev->dev, "failed to get io memory region\n");
+                ret = -ENODEV;
                 goto err_platform_get;
         }
 
@@ -405,6 +409,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
         dsim_ddi = exynos_mipi_dsi_bind_lcd_ddi(dsim, dsim_pd->lcd_panel_name);
         if (!dsim_ddi) {
                 dev_err(&pdev->dev, "mipi_dsim_ddi object not found.\n");
+                ret = -EINVAL;
                 goto err_bind;
         }
 
diff --git a/drivers/video/exynos/exynos_mipi_dsi_common.c b/drivers/video/exynos/exynos_mipi_dsi_common.c
index 47b533a183be..3cd29a4fc10a 100644
--- a/drivers/video/exynos/exynos_mipi_dsi_common.c
+++ b/drivers/video/exynos/exynos_mipi_dsi_common.c
@@ -79,11 +79,6 @@ irqreturn_t exynos_mipi_dsi_interrupt_handler(int irq, void *dev_id)
         struct mipi_dsim_device *dsim = dev_id;
         unsigned int intsrc, intmsk;
 
-        if (dsim == NULL) {
-                dev_err(dsim->dev, "%s: wrong parameter\n", __func__);
-                return IRQ_NONE;
-        }
-
         intsrc = exynos_mipi_dsi_read_interrupt(dsim);
         intmsk = exynos_mipi_dsi_read_interrupt_mask(dsim);
         intmsk = ~intmsk & intsrc;
@@ -288,9 +283,6 @@ int exynos_mipi_dsi_wr_data(struct mipi_dsim_device *dsim, unsigned int data_id,
                 mutex_unlock(&dsim->lock);
                 return -EINVAL;
         }
-
-        mutex_unlock(&dsim->lock);
-        return 0;
 }
 
 static unsigned int exynos_mipi_dsi_long_data_rd(struct mipi_dsim_device *dsim,
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 458c00664ade..ede9e55413f8 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -1501,8 +1501,8 @@ static int __devinit fsl_diu_probe(struct platform_device *pdev)
         unsigned int i;
         int ret;
 
-        data = dma_alloc_coherent(&pdev->dev, sizeof(struct fsl_diu_data),
-                                  &dma_addr, GFP_DMA | __GFP_ZERO);
+        data = dmam_alloc_coherent(&pdev->dev, sizeof(struct fsl_diu_data),
+                                   &dma_addr, GFP_DMA | __GFP_ZERO);
         if (!data)
                 return -ENOMEM;
         data->dma_addr = dma_addr;
@@ -1628,9 +1628,6 @@ error:
 
         iounmap(data->diu_reg);
 
-        dma_free_coherent(&pdev->dev, sizeof(struct fsl_diu_data), data,
-                          data->dma_addr);
-
         return ret;
 }
 
@@ -1648,9 +1645,6 @@ static int fsl_diu_remove(struct platform_device *pdev)
 
         iounmap(data->diu_reg);
 
-        dma_free_coherent(&pdev->dev, sizeof(struct fsl_diu_data), data,
-                          data->dma_addr);
-
         return 0;
 }
 
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index 05e2a8a99d8f..3dad31975db8 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -20,6 +20,7 @@
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/module.h>
+#include <linux/io.h>
 
 #ifdef CONFIG_X86
 #include <asm/mtrr.h>
@@ -28,7 +29,6 @@
 #include <asm/addrspace.h>
 #endif
 #include <asm/byteorder.h>
-#include <asm/io.h>
 #include <asm/tlbflush.h>
 
 #include <video/gbe.h>
@@ -1156,7 +1156,8 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
                 goto out_release_framebuffer;
         }
 
-        gbe = (struct sgi_gbe *) ioremap(GBE_BASE, sizeof(struct sgi_gbe));
+        gbe = (struct sgi_gbe *) devm_ioremap(&p_dev->dev, GBE_BASE,
+                                              sizeof(struct sgi_gbe));
         if (!gbe) {
                 printk(KERN_ERR "gbefb: couldn't map mmio region\n");
                 ret = -ENXIO;
@@ -1170,12 +1171,13 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
         if (!gbe_tiles.cpu) {
                 printk(KERN_ERR "gbefb: couldn't allocate tiles table\n");
                 ret = -ENOMEM;
-                goto out_unmap;
+                goto out_release_mem_region;
         }
 
         if (gbe_mem_phys) {
                 /* memory was allocated at boot time */
-                gbe_mem = ioremap_nocache(gbe_mem_phys, gbe_mem_size);
+                gbe_mem = devm_ioremap_nocache(&p_dev->dev, gbe_mem_phys,
+                                               gbe_mem_size);
                 if (!gbe_mem) {
                         printk(KERN_ERR "gbefb: couldn't map framebuffer\n");
                         ret = -ENOMEM;
@@ -1241,13 +1243,9 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
 out_gbe_unmap:
         if (gbe_dma_addr)
                 dma_free_coherent(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
-        else
-                iounmap(gbe_mem);
 out_tiles_free:
         dma_free_coherent(NULL, GBE_TLB_SIZE * sizeof(uint16_t),
                           (void *)gbe_tiles.cpu, gbe_tiles.dma);
-out_unmap:
-        iounmap(gbe);
 out_release_mem_region:
         release_mem_region(GBE_BASE, sizeof(struct sgi_gbe));
 out_release_framebuffer:
@@ -1264,12 +1262,9 @@ static int __devexit gbefb_remove(struct platform_device* p_dev)
         gbe_turn_off();
         if (gbe_dma_addr)
                 dma_free_coherent(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
-        else
-                iounmap(gbe_mem);
         dma_free_coherent(NULL, GBE_TLB_SIZE * sizeof(uint16_t),
                           (void *)gbe_tiles.cpu, gbe_tiles.dma);
         release_mem_region(GBE_BASE, sizeof(struct sgi_gbe));
-        iounmap(gbe);
         gbefb_remove_sysfs(&p_dev->dev);
         framebuffer_release(info);
 
diff --git a/drivers/video/hpfb.c b/drivers/video/hpfb.c
index ebf8495ff198..7324865f965f 100644
--- a/drivers/video/hpfb.c
+++ b/drivers/video/hpfb.c
@@ -210,6 +210,7 @@ static int __devinit hpfb_init_one(unsigned long phys_base,
                                    unsigned long virt_base)
 {
         unsigned long fboff, fb_width, fb_height, fb_start;
+        int ret;
 
         fb_regs = virt_base;
         fboff = (in_8(fb_regs + HPFB_FBOMSB) << 8) | in_8(fb_regs + HPFB_FBOLSB);
@@ -290,19 +291,29 @@ static int __devinit hpfb_init_one(unsigned long phys_base,
         fb_info.var   = hpfb_defined;
         fb_info.screen_base = (char *)fb_start;
 
-        fb_alloc_cmap(&fb_info.cmap, 1 << hpfb_defined.bits_per_pixel, 0);
+        ret = fb_alloc_cmap(&fb_info.cmap, 1 << hpfb_defined.bits_per_pixel, 0);
+        if (ret < 0)
+                goto unmap_screen_base;
 
-        if (register_framebuffer(&fb_info) < 0) {
-                fb_dealloc_cmap(&fb_info.cmap);
-                iounmap(fb_info.screen_base);
-                fb_info.screen_base = NULL;
-                return 1;
-        }
+        ret = register_framebuffer(&fb_info);
+        if (ret < 0)
+                goto dealloc_cmap;
 
         printk(KERN_INFO "fb%d: %s frame buffer device\n",
                fb_info.node, fb_info.fix.id);
 
         return 0;
+
+dealloc_cmap:
+        fb_dealloc_cmap(&fb_info.cmap);
+
+unmap_screen_base:
+        if (fb_info.screen_base) {
+                iounmap(fb_info.screen_base);
+                fb_info.screen_base = NULL;
+        }
+
+        return ret;
 }
 
 /* 
@@ -345,6 +356,9 @@ static void __devexit hpfb_remove_one(struct dio_dev *d)
         if (d->scode >= DIOII_SCBASE)
                 iounmap((void *)fb_regs);
         release_mem_region(d->resource.start, resource_size(&d->resource));
+        fb_dealloc_cmap(&fb_info.cmap);
+        if (fb_info.screen_base)
+                iounmap(fb_info.screen_base);
 }
 
 static struct dio_device_id hpfb_dio_tbl[] = {
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index 53ffdfc82a75..cf2688de0832 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -803,6 +803,7 @@ static int __init imxfb_probe(struct platform_device *pdev)
         fbi->regs = ioremap(res->start, resource_size(res));
         if (fbi->regs == NULL) {
                 dev_err(&pdev->dev, "Cannot map frame buffer registers\n");
+                ret = -ENOMEM;
                 goto failed_ioremap;
         }
 
diff --git a/drivers/video/jz4740_fb.c b/drivers/video/jz4740_fb.c
index 3c63fc24bb1f..4d25711b9982 100644
--- a/drivers/video/jz4740_fb.c
+++ b/drivers/video/jz4740_fb.c
@@ -632,23 +632,10 @@ static int __devinit jzfb_probe(struct platform_device *pdev)
                 return -ENXIO;
         }
 
-        mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!mem) {
-                dev_err(&pdev->dev, "Failed to get register memory resource\n");
-                return -ENXIO;
-        }
-
-        mem = request_mem_region(mem->start, resource_size(mem), pdev->name);
-        if (!mem) {
-                dev_err(&pdev->dev, "Failed to request register memory region\n");
-                return -EBUSY;
-        }
-
         fb = framebuffer_alloc(sizeof(struct jzfb), &pdev->dev);
         if (!fb) {
                 dev_err(&pdev->dev, "Failed to allocate framebuffer device\n");
-                ret = -ENOMEM;
-                goto err_release_mem_region;
+                return -ENOMEM;
         }
 
         fb->fbops = &jzfb_ops;
@@ -657,27 +644,26 @@ static int __devinit jzfb_probe(struct platform_device *pdev)
         jzfb = fb->par;
         jzfb->pdev = pdev;
         jzfb->pdata = pdata;
-        jzfb->mem = mem;
 
-        jzfb->ldclk = clk_get(&pdev->dev, "lcd");
+        jzfb->ldclk = devm_clk_get(&pdev->dev, "lcd");
         if (IS_ERR(jzfb->ldclk)) {
                 ret = PTR_ERR(jzfb->ldclk);
                 dev_err(&pdev->dev, "Failed to get lcd clock: %d\n", ret);
                 goto err_framebuffer_release;
         }
 
-        jzfb->lpclk = clk_get(&pdev->dev, "lcd_pclk");
+        jzfb->lpclk = devm_clk_get(&pdev->dev, "lcd_pclk");
         if (IS_ERR(jzfb->lpclk)) {
                 ret = PTR_ERR(jzfb->lpclk);
                 dev_err(&pdev->dev, "Failed to get lcd pixel clock: %d\n", ret);
-                goto err_put_ldclk;
+                goto err_framebuffer_release;
         }
 
-        jzfb->base = ioremap(mem->start, resource_size(mem));
+        mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        jzfb->base = devm_request_and_ioremap(&pdev->dev, mem);
         if (!jzfb->base) {
-                dev_err(&pdev->dev, "Failed to ioremap register memory region\n");
                 ret = -EBUSY;
-                goto err_put_lpclk;
+                goto err_framebuffer_release;
         }
 
         platform_set_drvdata(pdev, jzfb);
@@ -693,7 +679,7 @@ static int __devinit jzfb_probe(struct platform_device *pdev)
         ret = jzfb_alloc_devmem(jzfb);
         if (ret) {
                 dev_err(&pdev->dev, "Failed to allocate video memory\n");
-                goto err_iounmap;
+                goto err_framebuffer_release;
         }
 
         fb->fix = jzfb_fix;
@@ -734,16 +720,8 @@ err_free_devmem:
 
         fb_dealloc_cmap(&fb->cmap);
         jzfb_free_devmem(jzfb);
-err_iounmap:
-        iounmap(jzfb->base);
-err_put_lpclk:
-        clk_put(jzfb->lpclk);
-err_put_ldclk:
-        clk_put(jzfb->ldclk);
 err_framebuffer_release:
         framebuffer_release(fb);
-err_release_mem_region:
-        release_mem_region(mem->start, resource_size(mem));
         return ret;
 }
 
@@ -756,17 +734,11 @@ static int __devexit jzfb_remove(struct platform_device *pdev)
         jz_gpio_bulk_free(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
         jz_gpio_bulk_free(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
 
-        iounmap(jzfb->base);
-        release_mem_region(jzfb->mem->start, resource_size(jzfb->mem));
-
         fb_dealloc_cmap(&jzfb->fb->cmap);
         jzfb_free_devmem(jzfb);
 
         platform_set_drvdata(pdev, NULL);
 
-        clk_put(jzfb->lpclk);
-        clk_put(jzfb->ldclk);
-
         framebuffer_release(jzfb->fb);
 
         return 0;
diff --git a/drivers/video/mb862xx/mb862xxfbdrv.c b/drivers/video/mb862xx/mb862xxfbdrv.c
index 57d940be5f3d..d68e332aa21c 100644
--- a/drivers/video/mb862xx/mb862xxfbdrv.c
+++ b/drivers/video/mb862xx/mb862xxfbdrv.c
@@ -1052,12 +1052,14 @@ static int __devinit mb862xx_pci_probe(struct pci_dev *pdev,
                 break;
         default:
                 /* should never occur */
+                ret = -EIO;
                 goto rel_reg;
         }
 
         par->fb_base = ioremap(par->fb_base_phys, par->mapped_vram);
         if (par->fb_base == NULL) {
                 dev_err(dev, "Cannot map framebuffer\n");
+                ret = -EIO;
                 goto rel_reg;
         }
 
@@ -1073,11 +1075,13 @@ static int __devinit mb862xx_pci_probe(struct pci_dev *pdev,
         dev_dbg(dev, "mmio phys 0x%llx 0x%lx\n",
                 (unsigned long long)par->mmio_base_phys, (ulong)par->mmio_len);
 
-        if (mb862xx_pci_gdc_init(par))
+        ret = mb862xx_pci_gdc_init(par);
+        if (ret)
                 goto io_unmap;
 
-        if (request_irq(par->irq, mb862xx_intr, IRQF_SHARED,
-                        DRV_NAME, (void *)par)) {
+        ret = request_irq(par->irq, mb862xx_intr, IRQF_SHARED,
+                          DRV_NAME, (void *)par);
+        if (ret) {
                 dev_err(dev, "Cannot request irq\n");
                 goto io_unmap;
         }
diff --git a/drivers/video/mbx/mbxfb.c b/drivers/video/mbx/mbxfb.c
index 85e4f44bfa61..6563e50413c1 100644
--- a/drivers/video/mbx/mbxfb.c
+++ b/drivers/video/mbx/mbxfb.c
@@ -26,8 +26,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
-
-#include <asm/io.h>
+#include <linux/io.h>
 
 #include <video/mbxfb.h>
 
@@ -939,8 +938,9 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
         }
         mfbi->reg_phys_addr = mfbi->reg_res->start;
 
-        mfbi->reg_virt_addr = ioremap_nocache(mfbi->reg_phys_addr,
-                                              res_size(mfbi->reg_req));
+        mfbi->reg_virt_addr = devm_ioremap_nocache(&dev->dev,
+                                                   mfbi->reg_phys_addr,
+                                                   res_size(mfbi->reg_req));
         if (!mfbi->reg_virt_addr) {
                 dev_err(&dev->dev, "failed to ioremap Marathon registers\n");
                 ret = -EINVAL;
@@ -948,12 +948,12 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
         }
         virt_base_2700 = mfbi->reg_virt_addr;
 
-        mfbi->fb_virt_addr = ioremap_nocache(mfbi->fb_phys_addr,
-                                             res_size(mfbi->fb_req));
+        mfbi->fb_virt_addr = devm_ioremap_nocache(&dev->dev, mfbi->fb_phys_addr,
+                                                  res_size(mfbi->fb_req));
         if (!mfbi->fb_virt_addr) {
                 dev_err(&dev->dev, "failed to ioremap frame buffer\n");
                 ret = -EINVAL;
-                goto err4;
+                goto err3;
         }
 
         fbi->screen_base = (char __iomem *)(mfbi->fb_virt_addr + 0x60000);
@@ -971,7 +971,7 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
         if (ret < 0) {
                 dev_err(&dev->dev, "fb_alloc_cmap failed\n");
                 ret = -EINVAL;
-                goto err5;
+                goto err3;
         }
 
         platform_set_drvdata(dev, fbi);
@@ -996,10 +996,6 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
 
 err6:
         fb_dealloc_cmap(&fbi->cmap);
-err5:
-        iounmap(mfbi->fb_virt_addr);
-err4:
-        iounmap(mfbi->reg_virt_addr);
 err3:
         release_mem_region(mfbi->reg_res->start, res_size(mfbi->reg_res));
 err2:
@@ -1026,10 +1022,7 @@ static int __devexit mbxfb_remove(struct platform_device *dev)
                         if (mfbi->platform_remove)
                                 mfbi->platform_remove(fbi);
 
-                        if (mfbi->fb_virt_addr)
-                                iounmap(mfbi->fb_virt_addr);
-                        if (mfbi->reg_virt_addr)
-                                iounmap(mfbi->reg_virt_addr);
+
                         if (mfbi->reg_req)
                                 release_mem_region(mfbi->reg_req->start,
                                                    res_size(mfbi->reg_req));
diff --git a/drivers/video/msm/mddi.c b/drivers/video/msm/mddi.c
index bf73f0480061..35ac9e8bee63 100644
--- a/drivers/video/msm/mddi.c
+++ b/drivers/video/msm/mddi.c
@@ -26,9 +26,6 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/sched.h>
-#include <mach/msm_iomap.h>
-#include <mach/irqs.h>
-#include <mach/board.h>
 #include <linux/platform_data/video-msm_fb.h>
 #include "mddi_hw.h"
 
diff --git a/drivers/video/msm/mddi_client_nt35399.c b/drivers/video/msm/mddi_client_nt35399.c
index d7a5bf84fb2a..f96df32e5509 100644
--- a/drivers/video/msm/mddi_client_nt35399.c
+++ b/drivers/video/msm/mddi_client_nt35399.c
@@ -189,8 +189,9 @@ static int mddi_nt35399_probe(struct platform_device *pdev)
 
         int ret;
 
-        struct panel_info *panel = kzalloc(sizeof(struct panel_info),
-                                           GFP_KERNEL);
+        struct panel_info *panel = devm_kzalloc(&pdev->dev,
+                                                sizeof(struct panel_info),
+                                                GFP_KERNEL);
 
         printk(KERN_DEBUG "%s: enter.\n", __func__);
 
@@ -233,7 +234,6 @@ static int mddi_nt35399_remove(struct platform_device *pdev)
         struct panel_info *panel = platform_get_drvdata(pdev);
 
         setup_vsync(panel, 0);
-        kfree(panel);
         return 0;
 }
 
diff --git a/drivers/video/msm/mdp.c b/drivers/video/msm/mdp.c
index 2e0f3bab6114..f2566c19e71c 100644
--- a/drivers/video/msm/mdp.c
+++ b/drivers/video/msm/mdp.c
@@ -25,7 +25,6 @@
 #include <linux/major.h>
 #include <linux/slab.h>
 
-#include <mach/msm_iomap.h>
 #include <linux/platform_data/video-msm_fb.h>
 #include <linux/platform_device.h>
 #include <linux/export.h>
diff --git a/drivers/video/msm/mdp_hw.h b/drivers/video/msm/mdp_hw.h
index a0bacf581b32..35848d741001 100644
--- a/drivers/video/msm/mdp_hw.h
+++ b/drivers/video/msm/mdp_hw.h
@@ -15,7 +15,6 @@
 #ifndef _MDP_HW_H_
 #define _MDP_HW_H_
 
-#include <mach/msm_iomap.h>
 #include <linux/platform_data/video-msm_fb.h>
 
 struct mdp_info {
diff --git a/drivers/video/mx3fb.c b/drivers/video/mx3fb.c
index d7381088a180..ce1d452464ed 100644
--- a/drivers/video/mx3fb.c
+++ b/drivers/video/mx3fb.c
@@ -1568,7 +1568,8 @@ static int mx3fb_remove(struct platform_device *dev)
 
 static struct platform_driver mx3fb_driver = {
         .driver = {
-                   .name = MX3FB_NAME,
+                .name = MX3FB_NAME,
+                .owner = THIS_MODULE,
         },
         .probe = mx3fb_probe,
         .remove = mx3fb_remove,
diff --git a/drivers/video/nuc900fb.c b/drivers/video/nuc900fb.c
index 93387555337e..475dfee82c4a 100644
--- a/drivers/video/nuc900fb.c
+++ b/drivers/video/nuc900fb.c
@@ -387,7 +387,7 @@ static int nuc900fb_init_registers(struct fb_info *info)
  *    The buffer should be a non-cached, non-buffered, memory region
  *    to allow palette and pixel writes without flushing the cache.
  */
-static int __init nuc900fb_map_video_memory(struct fb_info *info)
+static int __devinit nuc900fb_map_video_memory(struct fb_info *info)
 {
         struct nuc900fb_info *fbi = info->par;
         dma_addr_t map_dma;
diff --git a/drivers/video/omap/hwa742.c b/drivers/video/omap/hwa742.c
index 9f1d23c319cb..f349ee6f0cea 100644
--- a/drivers/video/omap/hwa742.c
+++ b/drivers/video/omap/hwa742.c
@@ -27,7 +27,6 @@
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 
-#include <plat/dma.h>
 #include "omapfb.h"
 
 #define HWA742_REV_CODE_REG       0x0
diff --git a/drivers/video/omap/lcd_palmte.c b/drivers/video/omap/lcd_palmte.c
index 88c31eb0cd6c..ff4fb624b904 100644
--- a/drivers/video/omap/lcd_palmte.c
+++ b/drivers/video/omap/lcd_palmte.c
@@ -23,7 +23,6 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 
-#include <plat/fpga.h>
 #include "omapfb.h"
 
 static int palmte_panel_init(struct lcd_panel *panel,
diff --git a/drivers/video/omap/omapfb_main.c b/drivers/video/omap/omapfb_main.c
index f54b463709e9..4351c438b76f 100644
--- a/drivers/video/omap/omapfb_main.c
+++ b/drivers/video/omap/omapfb_main.c
@@ -131,15 +131,6 @@ static void omapfb_rqueue_unlock(struct omapfb_device *fbdev)
  * LCD controller and LCD DMA
  * ---------------------------------------------------------------------------
  */
-/* Lookup table to map elem size to elem type. */
-static const int dma_elem_type[] = {
-        0,
-        OMAP_DMA_DATA_TYPE_S8,
-        OMAP_DMA_DATA_TYPE_S16,
-        0,
-        OMAP_DMA_DATA_TYPE_S32,
-};
-
 /*
  * Allocate resources needed for LCD controller and LCD DMA operations. Video
  * memory is allocated from system memory according to the virtual display
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index eaeed4340e04..c835aa70f96f 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -600,6 +600,9 @@ static int acx_panel_power_on(struct omap_dss_device *dssdev)
 
         mutex_lock(&md->mutex);
 
+        omapdss_sdi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_sdi_set_datapairs(dssdev, dssdev->phy.sdi.datapairs);
+
         r = omapdss_sdi_display_enable(dssdev);
         if (r) {
                 pr_err("%s sdi enable failed\n", __func__);
@@ -731,18 +734,9 @@ static int acx_panel_resume(struct omap_dss_device *dssdev)
 static void acx_panel_set_timings(struct omap_dss_device *dssdev,
                 struct omap_video_timings *timings)
 {
-        int r;
-
-        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
-                omapdss_sdi_display_disable(dssdev);
+        omapdss_sdi_set_timings(dssdev, timings);
 
         dssdev->panel.timings = *timings;
-
-        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-                r = omapdss_sdi_display_enable(dssdev);
-                if (r)
-                        dev_err(&dssdev->dev, "%s enable failed\n", __func__);
-        }
 }
 
 static int acx_panel_check_timings(struct omap_dss_device *dssdev,
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index bc5af2500eb9..88295c526815 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -545,6 +545,8 @@ struct panel_drv_data {
         struct omap_dss_device *dssdev;
 
         struct panel_config *panel_config;
+
+        struct mutex lock;
 };
 
 static inline struct panel_generic_dpi_data
@@ -563,6 +565,9 @@ static int generic_dpi_panel_power_on(struct omap_dss_device *dssdev)
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
@@ -634,6 +639,8 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
         drv_data->dssdev = dssdev;
         drv_data->panel_config = panel_config;
 
+        mutex_init(&drv_data->lock);
+
         dev_set_drvdata(&dssdev->dev, drv_data);
 
         return 0;
@@ -652,56 +659,108 @@ static void __exit generic_dpi_panel_remove(struct omap_dss_device *dssdev)
 
 static int generic_dpi_panel_enable(struct omap_dss_device *dssdev)
 {
-        int r = 0;
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&drv_data->lock);
 
         r = generic_dpi_panel_power_on(dssdev);
         if (r)
-                return r;
+                goto err;
 
         dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+err:
+        mutex_unlock(&drv_data->lock);
 
-        return 0;
+        return r;
 }
 
 static void generic_dpi_panel_disable(struct omap_dss_device *dssdev)
 {
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&drv_data->lock);
+
         generic_dpi_panel_power_off(dssdev);
 
         dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        mutex_unlock(&drv_data->lock);
 }
 
 static int generic_dpi_panel_suspend(struct omap_dss_device *dssdev)
 {
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&drv_data->lock);
+
         generic_dpi_panel_power_off(dssdev);
 
         dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
+        mutex_unlock(&drv_data->lock);
+
         return 0;
 }
 
 static int generic_dpi_panel_resume(struct omap_dss_device *dssdev)
 {
-        int r = 0;
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&drv_data->lock);
 
         r = generic_dpi_panel_power_on(dssdev);
         if (r)
-                return r;
+                goto err;
 
         dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
-        return 0;
+err:
+        mutex_unlock(&drv_data->lock);
+
+        return r;
 }
 
 static void generic_dpi_panel_set_timings(struct omap_dss_device *dssdev,
                 struct omap_video_timings *timings)
 {
-        dpi_set_timings(dssdev, timings);
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&drv_data->lock);
+
+        omapdss_dpi_set_timings(dssdev, timings);
+
+        dssdev->panel.timings = *timings;
+
+        mutex_unlock(&drv_data->lock);
+}
+
+static void generic_dpi_panel_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&drv_data->lock);
+
+        *timings = dssdev->panel.timings;
+
+        mutex_unlock(&drv_data->lock);
 }
 
 static int generic_dpi_panel_check_timings(struct omap_dss_device *dssdev,
                 struct omap_video_timings *timings)
 {
-        return dpi_check_timings(dssdev, timings);
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&drv_data->lock);
+
+        r = dpi_check_timings(dssdev, timings);
+
+        mutex_unlock(&drv_data->lock);
+
+        return r;
 }
 
 static struct omap_dss_driver dpi_driver = {
@@ -714,6 +773,7 @@ static struct omap_dss_driver dpi_driver = {
         .resume         = generic_dpi_panel_resume,
 
         .set_timings    = generic_dpi_panel_set_timings,
+        .get_timings    = generic_dpi_panel_get_timings,
         .check_timings  = generic_dpi_panel_check_timings,
 
         .driver         = {
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
index 802807798846..90c1cabf244e 100644
--- a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -55,6 +55,9 @@ static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
diff --git a/drivers/video/omap2/displays/panel-n8x0.c b/drivers/video/omap2/displays/panel-n8x0.c
index e6c115373c00..3fc5ad081a21 100644
--- a/drivers/video/omap2/displays/panel-n8x0.c
+++ b/drivers/video/omap2/displays/panel-n8x0.c
@@ -150,11 +150,17 @@ static void blizzard_ctrl_setup_update(struct omap_dss_device *dssdev,
                         BLIZZARD_SRC_WRITE_LCD :
                         BLIZZARD_SRC_WRITE_LCD_DESTRUCTIVE;
 
-        omap_rfbi_configure(dssdev, 16, 8);
+        omapdss_rfbi_set_pixel_size(dssdev, 16);
+        omapdss_rfbi_set_data_lines(dssdev, 8);
+
+        omap_rfbi_configure(dssdev);
 
         blizzard_write(BLIZZARD_INPUT_WIN_X_START_0, tmp, 18);
 
-        omap_rfbi_configure(dssdev, 16, 16);
+        omapdss_rfbi_set_pixel_size(dssdev, 16);
+        omapdss_rfbi_set_data_lines(dssdev, 16);
+
+        omap_rfbi_configure(dssdev);
 }
 
 static void mipid_transfer(struct spi_device *spi, int cmd, const u8 *wbuf,
@@ -297,6 +303,12 @@ static int n8x0_panel_power_on(struct omap_dss_device *dssdev)
                         goto err_plat_en;
         }
 
+        omapdss_rfbi_set_size(dssdev, dssdev->panel.timings.x_res,
+                dssdev->panel.timings.y_res);
+        omapdss_rfbi_set_pixel_size(dssdev, dssdev->ctrl.pixel_size);
+        omapdss_rfbi_set_data_lines(dssdev, dssdev->phy.rfbi.data_lines);
+        omapdss_rfbi_set_interface_timings(dssdev, &dssdev->ctrl.rfbi_timings);
+
         r = omapdss_rfbi_display_enable(dssdev);
         if (r)
                 goto err_rfbi_en;
@@ -477,6 +489,7 @@ static int n8x0_panel_probe(struct omap_dss_device *dssdev)
         dssdev->panel.timings.y_res = 480;
         dssdev->ctrl.pixel_size = 16;
         dssdev->ctrl.rfbi_timings = n8x0_panel_timings;
+        dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 
         memset(&props, 0, sizeof(props));
         props.max_brightness = 127;
@@ -625,17 +638,25 @@ static int n8x0_panel_update(struct omap_dss_device *dssdev,
                 u16 x, u16 y, u16 w, u16 h)
 {
         struct panel_drv_data *ddata = get_drv_data(dssdev);
+        u16 dw, dh;
 
         dev_dbg(&dssdev->dev, "update\n");
 
+        dw = dssdev->panel.timings.x_res;
+        dh = dssdev->panel.timings.y_res;
+
+        if (x != 0 || y != 0 || w != dw || h != dh) {
+                dev_err(&dssdev->dev, "invaid update region %d, %d, %d, %d\n",
+                        x, y, w, h);
+                return -EINVAL;
+        }
+
         mutex_lock(&ddata->lock);
         rfbi_bus_lock();
 
-        omap_rfbi_prepare_update(dssdev, &x, &y, &w, &h);
-
         blizzard_ctrl_setup_update(dssdev, x, y, w, h);
 
-        omap_rfbi_update(dssdev, x, y, w, h, update_done, NULL);
+        omap_rfbi_update(dssdev, update_done, NULL);
 
         mutex_unlock(&ddata->lock);
 
diff --git a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
index b122b0f31c43..908fd268f3dc 100644
--- a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
+++ b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
@@ -175,6 +175,9 @@ static int nec_8048_panel_power_on(struct omap_dss_device *dssdev)
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
diff --git a/drivers/video/omap2/displays/panel-picodlp.c b/drivers/video/omap2/displays/panel-picodlp.c
index 2d35bd388860..9df87640ddd2 100644
--- a/drivers/video/omap2/displays/panel-picodlp.c
+++ b/drivers/video/omap2/displays/panel-picodlp.c
@@ -377,6 +377,10 @@ static int picodlp_panel_power_on(struct omap_dss_device *dssdev)
          * then only i2c commands can be successfully sent to dpp2600
          */
         msleep(1000);
+
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r) {
                 dev_err(&dssdev->dev, "failed to enable DPI\n");
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index bd86ba9ccf76..1ec3b277ff15 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -142,6 +142,9 @@ static int sharp_ls_power_on(struct omap_dss_device *dssdev)
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index 6b5e6e0e202f..f2f644680ca8 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -121,6 +121,18 @@ struct taal_data {
 
         struct omap_dss_device *dssdev;
 
+        /* panel specific HW info */
+        struct panel_config *panel_config;
+
+        /* panel HW configuration from DT or platform data */
+        int reset_gpio;
+        int ext_te_gpio;
+
+        bool use_dsi_backlight;
+
+        struct omap_dsi_pin_config pin_config;
+
+        /* runtime variables */
         bool enabled;
         u8 rotate;
         bool mirror;
@@ -145,16 +157,8 @@ struct taal_data {
         bool ulps_enabled;
         unsigned ulps_timeout;
         struct delayed_work ulps_work;
-
-        struct panel_config *panel_config;
 };
 
-static inline struct nokia_dsi_panel_data
-*get_panel_data(const struct omap_dss_device *dssdev)
-{
-        return (struct nokia_dsi_panel_data *) dssdev->data;
-}
-
 static void taal_esd_work(struct work_struct *work);
 static void taal_ulps_work(struct work_struct *work);
 
@@ -371,7 +375,6 @@ static void taal_cancel_ulps_work(struct omap_dss_device *dssdev)
 static int taal_enter_ulps(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         if (td->ulps_enabled)
@@ -383,7 +386,8 @@ static int taal_enter_ulps(struct omap_dss_device *dssdev)
         if (r)
                 goto err;
 
-        disable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+        if (gpio_is_valid(td->ext_te_gpio))
+                disable_irq(gpio_to_irq(td->ext_te_gpio));
 
         omapdss_dsi_display_disable(dssdev, false, true);
 
@@ -405,7 +409,6 @@ err:
 static int taal_exit_ulps(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         if (!td->ulps_enabled)
@@ -425,7 +428,8 @@ static int taal_exit_ulps(struct omap_dss_device *dssdev)
                 goto err2;
         }
 
-        enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+        if (gpio_is_valid(td->ext_te_gpio))
+                enable_irq(gpio_to_irq(td->ext_te_gpio));
 
         taal_queue_ulps_work(dssdev);
 
@@ -438,7 +442,8 @@ err2:
 
         r = taal_panel_reset(dssdev);
         if (!r) {
-                enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+                if (gpio_is_valid(td->ext_te_gpio))
+                        enable_irq(gpio_to_irq(td->ext_te_gpio));
                 td->ulps_enabled = false;
         }
 err1:
@@ -835,94 +840,135 @@ static struct attribute_group taal_attr_group = {
 static void taal_hw_reset(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 
-        if (panel_data->reset_gpio == -1)
+        if (!gpio_is_valid(td->reset_gpio))
                 return;
 
-        gpio_set_value(panel_data->reset_gpio, 1);
+        gpio_set_value(td->reset_gpio, 1);
         if (td->panel_config->reset_sequence.high)
                 udelay(td->panel_config->reset_sequence.high);
         /* reset the panel */
-        gpio_set_value(panel_data->reset_gpio, 0);
+        gpio_set_value(td->reset_gpio, 0);
         /* assert reset */
         if (td->panel_config->reset_sequence.low)
                 udelay(td->panel_config->reset_sequence.low);
-        gpio_set_value(panel_data->reset_gpio, 1);
+        gpio_set_value(td->reset_gpio, 1);
         /* wait after releasing reset */
         if (td->panel_config->sleep.hw_reset)
                 msleep(td->panel_config->sleep.hw_reset);
 }
 
+static void taal_probe_pdata(struct taal_data *td,
+                const struct nokia_dsi_panel_data *pdata)
+{
+        td->reset_gpio = pdata->reset_gpio;
+
+        if (pdata->use_ext_te)
+                td->ext_te_gpio = pdata->ext_te_gpio;
+        else
+                td->ext_te_gpio = -1;
+
+        td->esd_interval = pdata->esd_interval;
+        td->ulps_timeout = pdata->ulps_timeout;
+
+        td->use_dsi_backlight = pdata->use_dsi_backlight;
+
+        td->pin_config = pdata->pin_config;
+}
+
 static int taal_probe(struct omap_dss_device *dssdev)
 {
         struct backlight_properties props;
         struct taal_data *td;
         struct backlight_device *bldev = NULL;
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
-        struct panel_config *panel_config = NULL;
         int r, i;
+        const char *panel_name;
 
         dev_dbg(&dssdev->dev, "probe\n");
 
-        if (!panel_data || !panel_data->name) {
-                r = -EINVAL;
-                goto err;
+        td = devm_kzalloc(&dssdev->dev, sizeof(*td), GFP_KERNEL);
+        if (!td)
+                return -ENOMEM;
+
+        dev_set_drvdata(&dssdev->dev, td);
+        td->dssdev = dssdev;
+
+        if (dssdev->data) {
+                const struct nokia_dsi_panel_data *pdata = dssdev->data;
+
+                taal_probe_pdata(td, pdata);
+
+                panel_name = pdata->name;
+        } else {
+                return -ENODEV;
         }
 
+        if (panel_name == NULL)
+                return -EINVAL;
+
         for (i = 0; i < ARRAY_SIZE(panel_configs); i++) {
-                if (strcmp(panel_data->name, panel_configs[i].name) == 0) {
-                        panel_config = &panel_configs[i];
+                if (strcmp(panel_name, panel_configs[i].name) == 0) {
+                        td->panel_config = &panel_configs[i];
                         break;
                 }
         }
 
-        if (!panel_config) {
-                r = -EINVAL;
-                goto err;
-        }
+        if (!td->panel_config)
+                return -EINVAL;
 
-        dssdev->panel.timings = panel_config->timings;
+        dssdev->panel.timings = td->panel_config->timings;
         dssdev->panel.dsi_pix_fmt = OMAP_DSS_DSI_FMT_RGB888;
-
-        td = kzalloc(sizeof(*td), GFP_KERNEL);
-        if (!td) {
-                r = -ENOMEM;
-                goto err;
-        }
-        td->dssdev = dssdev;
-        td->panel_config = panel_config;
-        td->esd_interval = panel_data->esd_interval;
-        td->ulps_enabled = false;
-        td->ulps_timeout = panel_data->ulps_timeout;
+        dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
+                OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
 
         mutex_init(&td->lock);
 
         atomic_set(&td->do_update, 0);
 
-        td->workqueue = create_singlethread_workqueue("taal_esd");
-        if (td->workqueue == NULL) {
-                dev_err(&dssdev->dev, "can't create ESD workqueue\n");
-                r = -ENOMEM;
-                goto err_wq;
+        if (gpio_is_valid(td->reset_gpio)) {
+                r = devm_gpio_request_one(&dssdev->dev, td->reset_gpio,
+                                GPIOF_OUT_INIT_LOW, "taal rst");
+                if (r) {
+                        dev_err(&dssdev->dev, "failed to request reset gpio\n");
+                        return r;
+                }
         }
-        INIT_DEFERRABLE_WORK(&td->esd_work, taal_esd_work);
-        INIT_DELAYED_WORK(&td->ulps_work, taal_ulps_work);
 
-        dev_set_drvdata(&dssdev->dev, td);
+        if (gpio_is_valid(td->ext_te_gpio)) {
+                r = devm_gpio_request_one(&dssdev->dev, td->ext_te_gpio,
+                                GPIOF_IN, "taal irq");
+                if (r) {
+                        dev_err(&dssdev->dev, "GPIO request failed\n");
+                        return r;
+                }
+
+                r = devm_request_irq(&dssdev->dev, gpio_to_irq(td->ext_te_gpio),
+                                taal_te_isr,
+                                IRQF_TRIGGER_RISING,
+                                "taal vsync", dssdev);
 
-        if (gpio_is_valid(panel_data->reset_gpio)) {
-                r = gpio_request_one(panel_data->reset_gpio, GPIOF_OUT_INIT_LOW,
-                                "taal rst");
                 if (r) {
-                        dev_err(&dssdev->dev, "failed to request reset gpio\n");
-                        goto err_rst_gpio;
+                        dev_err(&dssdev->dev, "IRQ request failed\n");
+                        return r;
                 }
+
+                INIT_DEFERRABLE_WORK(&td->te_timeout_work,
+                                        taal_te_timeout_work_callback);
+
+                dev_dbg(&dssdev->dev, "Using GPIO TE\n");
         }
 
+        td->workqueue = create_singlethread_workqueue("taal_esd");
+        if (td->workqueue == NULL) {
+                dev_err(&dssdev->dev, "can't create ESD workqueue\n");
+                return -ENOMEM;
+        }
+        INIT_DEFERRABLE_WORK(&td->esd_work, taal_esd_work);
+        INIT_DELAYED_WORK(&td->ulps_work, taal_ulps_work);
+
         taal_hw_reset(dssdev);
 
-        if (panel_data->use_dsi_backlight) {
+        if (td->use_dsi_backlight) {
                 memset(&props, 0, sizeof(struct backlight_properties));
                 props.max_brightness = 255;
 
@@ -943,31 +989,6 @@ static int taal_probe(struct omap_dss_device *dssdev)
                 taal_bl_update_status(bldev);
         }
 
-        if (panel_data->use_ext_te) {
-                int gpio = panel_data->ext_te_gpio;
-
-                r = gpio_request_one(gpio, GPIOF_IN, "taal irq");
-                if (r) {
-                        dev_err(&dssdev->dev, "GPIO request failed\n");
-                        goto err_gpio;
-                }
-
-                r = request_irq(gpio_to_irq(gpio), taal_te_isr,
-                                IRQF_TRIGGER_RISING,
-                                "taal vsync", dssdev);
-
-                if (r) {
-                        dev_err(&dssdev->dev, "IRQ request failed\n");
-                        gpio_free(gpio);
-                        goto err_irq;
-                }
-
-                INIT_DEFERRABLE_WORK(&td->te_timeout_work,
-                                     taal_te_timeout_work_callback);
-
-                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");
@@ -991,29 +1012,16 @@ static int taal_probe(struct omap_dss_device *dssdev)
 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:
-        if (panel_data->use_ext_te)
-                gpio_free(panel_data->ext_te_gpio);
-err_gpio:
         if (bldev != NULL)
                 backlight_device_unregister(bldev);
 err_bl:
-        if (gpio_is_valid(panel_data->reset_gpio))
-                gpio_free(panel_data->reset_gpio);
-err_rst_gpio:
         destroy_workqueue(td->workqueue);
-err_wq:
-        kfree(td);
-err:
         return r;
 }
 
 static void __exit taal_remove(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         struct backlight_device *bldev;
 
         dev_dbg(&dssdev->dev, "remove\n");
@@ -1021,12 +1029,6 @@ static void __exit taal_remove(struct omap_dss_device *dssdev)
         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;
-                free_irq(gpio_to_irq(gpio), dssdev);
-                gpio_free(gpio);
-        }
-
         bldev = td->bldev;
         if (bldev != NULL) {
                 bldev->props.power = FB_BLANK_POWERDOWN;
@@ -1040,26 +1042,31 @@ static void __exit taal_remove(struct omap_dss_device *dssdev)
 
         /* reset, to be sure that the panel is in a valid state */
         taal_hw_reset(dssdev);
-
-        if (gpio_is_valid(panel_data->reset_gpio))
-                gpio_free(panel_data->reset_gpio);
-
-        kfree(td);
 }
 
 static int taal_power_on(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         u8 id1, id2, id3;
         int r;
 
-        r = omapdss_dsi_configure_pins(dssdev, &panel_data->pin_config);
+        r = omapdss_dsi_configure_pins(dssdev, &td->pin_config);
         if (r) {
                 dev_err(&dssdev->dev, "failed to configure DSI pins\n");
                 goto err0;
         };
 
+        omapdss_dsi_set_size(dssdev, dssdev->panel.timings.x_res,
+                dssdev->panel.timings.y_res);
+        omapdss_dsi_set_pixel_format(dssdev, OMAP_DSS_DSI_FMT_RGB888);
+        omapdss_dsi_set_operation_mode(dssdev, OMAP_DSS_DSI_CMD_MODE);
+
+        r = omapdss_dsi_set_clocks(dssdev, 216000000, 10000000);
+        if (r) {
+                dev_err(&dssdev->dev, "failed to set HS and LP clocks\n");
+                goto err0;
+        }
+
         r = omapdss_dsi_display_enable(dssdev);
         if (r) {
                 dev_err(&dssdev->dev, "failed to enable DSI\n");
@@ -1356,7 +1363,6 @@ static int taal_update(struct omap_dss_device *dssdev,
                                     u16 x, u16 y, u16 w, u16 h)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         dev_dbg(&dssdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
@@ -1380,7 +1386,7 @@ static int taal_update(struct omap_dss_device *dssdev,
         if (r)
                 goto err;
 
-        if (td->te_enabled && panel_data->use_ext_te) {
+        if (td->te_enabled && gpio_is_valid(td->ext_te_gpio)) {
                 schedule_delayed_work(&td->te_timeout_work,
                                 msecs_to_jiffies(250));
                 atomic_set(&td->do_update, 1);
@@ -1419,7 +1425,6 @@ static int taal_sync(struct omap_dss_device *dssdev)
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         if (enable)
@@ -1427,7 +1432,7 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
         else
                 r = taal_dcs_write_0(td, MIPI_DCS_SET_TEAR_OFF);
 
-        if (!panel_data->use_ext_te)
+        if (!gpio_is_valid(td->ext_te_gpio))
                 omapdss_dsi_enable_te(dssdev, enable);
 
         if (td->panel_config->sleep.enable_te)
@@ -1487,6 +1492,7 @@ static int taal_get_te(struct omap_dss_device *dssdev)
 static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        u16 dw, dh;
         int r;
 
         dev_dbg(&dssdev->dev, "rotate %d\n", rotate);
@@ -1508,6 +1514,16 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
                         goto err;
         }
 
+        if (rotate == 0 || rotate == 2) {
+                dw = dssdev->panel.timings.x_res;
+                dh = dssdev->panel.timings.y_res;
+        } else {
+                dw = dssdev->panel.timings.y_res;
+                dh = dssdev->panel.timings.x_res;
+        }
+
+        omapdss_dsi_set_size(dssdev, dw, dh);
+
         td->rotate = rotate;
 
         dsi_bus_unlock(dssdev);
@@ -1726,7 +1742,6 @@ static void taal_esd_work(struct work_struct *work)
         struct taal_data *td = container_of(work, struct taal_data,
                         esd_work.work);
         struct omap_dss_device *dssdev = td->dssdev;
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         u8 state1, state2;
         int r;
 
@@ -1773,7 +1788,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) {
+        if (td->te_enabled && gpio_is_valid(td->ext_te_gpio)) {
                 r = taal_dcs_write_1(td, MIPI_DCS_SET_TEAR_ON, 0);
                 if (r)
                         goto err;
diff --git a/drivers/video/omap2/displays/panel-tfp410.c b/drivers/video/omap2/displays/panel-tfp410.c
index 40cc0cfa5d17..383811cf8648 100644
--- a/drivers/video/omap2/displays/panel-tfp410.c
+++ b/drivers/video/omap2/displays/panel-tfp410.c
@@ -65,6 +65,9 @@ static int tfp410_power_on(struct omap_dss_device *dssdev)
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
@@ -116,8 +119,8 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
         }
 
         if (gpio_is_valid(ddata->pd_gpio)) {
-                r = gpio_request_one(ddata->pd_gpio, GPIOF_OUT_INIT_LOW,
-                                "tfp410 pd");
+                r = devm_gpio_request_one(&dssdev->dev, ddata->pd_gpio,
+                                GPIOF_OUT_INIT_LOW, "tfp410 pd");
                 if (r) {
                         dev_err(&dssdev->dev, "Failed to request PD GPIO %d\n",
                                         ddata->pd_gpio);
@@ -132,8 +135,7 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
                 if (!adapter) {
                         dev_err(&dssdev->dev, "Failed to get I2C adapter, bus %d\n",
                                         i2c_bus_num);
-                        r = -EINVAL;
-                        goto err_i2c;
+                        return -EINVAL;
                 }
 
                 ddata->i2c_adapter = adapter;
@@ -142,10 +144,6 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
         dev_set_drvdata(&dssdev->dev, ddata);
 
         return 0;
-err_i2c:
-        if (gpio_is_valid(ddata->pd_gpio))
-                gpio_free(ddata->pd_gpio);
-        return r;
 }
 
 static void __exit tfp410_remove(struct omap_dss_device *dssdev)
@@ -157,9 +155,6 @@ static void __exit tfp410_remove(struct omap_dss_device *dssdev)
         if (ddata->i2c_adapter)
                 i2c_put_adapter(ddata->i2c_adapter);
 
-        if (gpio_is_valid(ddata->pd_gpio))
-                gpio_free(ddata->pd_gpio);
-
         dev_set_drvdata(&dssdev->dev, NULL);
 
         mutex_unlock(&ddata->lock);
@@ -231,7 +226,8 @@ static void tfp410_set_timings(struct omap_dss_device *dssdev,
         struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 
         mutex_lock(&ddata->lock);
-        dpi_set_timings(dssdev, timings);
+        omapdss_dpi_set_timings(dssdev, timings);
+        dssdev->panel.timings = *timings;
         mutex_unlock(&ddata->lock);
 }
 
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index fa7baa650ae0..b5e6dbc59f0a 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -337,6 +337,9 @@ static int tpo_td043_enable_dss(struct omap_dss_device *dssdev)
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
+        omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
@@ -480,7 +483,9 @@ static void tpo_td043_remove(struct omap_dss_device *dssdev)
 static void tpo_td043_set_timings(struct omap_dss_device *dssdev,
                 struct omap_video_timings *timings)
 {
-        dpi_set_timings(dssdev, timings);
+        omapdss_dpi_set_timings(dssdev, timings);
+
+        dssdev->panel.timings = *timings;
 }
 
 static int tpo_td043_check_timings(struct omap_dss_device *dssdev,
diff --git a/drivers/video/omap2/dss/Kconfig b/drivers/video/omap2/dss/Kconfig
index b337a8469fd8..80f5390aa136 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -84,7 +84,7 @@ config OMAP2_DSS_SDI
 
 config OMAP2_DSS_DSI
         bool "DSI support"
-        depends on ARCH_OMAP3 || ARCH_OMAP4
+        depends on ARCH_OMAP3 || ARCH_OMAP4 || ARCH_OMAP5
         default n
         help
           MIPI DSI (Display Serial Interface) support.
diff --git a/drivers/video/omap2/dss/Makefile b/drivers/video/omap2/dss/Makefile
index 5c450b0f94d0..4549869bfe1a 100644
--- a/drivers/video/omap2/dss/Makefile
+++ b/drivers/video/omap2/dss/Makefile
@@ -1,9 +1,9 @@
 obj-$(CONFIG_OMAP2_DSS) += omapdss.o
 omapdss-y := core.o dss.o dss_features.o dispc.o dispc_coefs.o display.o \
-        manager.o overlay.o apply.o
+        manager.o manager-sysfs.o overlay.o overlay-sysfs.o output.o apply.o
 omapdss-$(CONFIG_OMAP2_DSS_DPI) += dpi.o
 omapdss-$(CONFIG_OMAP2_DSS_RFBI) += rfbi.o
-omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o
+omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o venc_panel.o
 omapdss-$(CONFIG_OMAP2_DSS_SDI) += sdi.o
 omapdss-$(CONFIG_OMAP2_DSS_DSI) += dsi.o
 omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi.o \
diff --git a/drivers/video/omap2/dss/apply.c b/drivers/video/omap2/dss/apply.c
index 0fefc68372b9..19d66f471b4b 100644
--- a/drivers/video/omap2/dss/apply.c
+++ b/drivers/video/omap2/dss/apply.c
@@ -111,9 +111,6 @@ static struct {
         struct ovl_priv_data ovl_priv_data_array[MAX_DSS_OVERLAYS];
         struct mgr_priv_data mgr_priv_data_array[MAX_DSS_MANAGERS];
 
-        bool fifo_merge_dirty;
-        bool fifo_merge;
-
         bool irq_enabled;
 } dss_data;
 
@@ -424,17 +421,25 @@ static void wait_pending_extra_info_updates(void)
 int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 {
         unsigned long timeout = msecs_to_jiffies(500);
-        struct mgr_priv_data *mp;
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
         u32 irq;
+        unsigned long flags;
         int r;
         int i;
-        struct omap_dss_device *dssdev = mgr->device;
 
-        if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (mgr_manual_update(mgr)) {
+                spin_unlock_irqrestore(&data_lock, flags);
                 return 0;
+        }
 
-        if (mgr_manual_update(mgr))
+        if (!mp->enabled) {
+                spin_unlock_irqrestore(&data_lock, flags);
                 return 0;
+        }
+
+        spin_unlock_irqrestore(&data_lock, flags);
 
         r = dispc_runtime_get();
         if (r)
@@ -442,10 +447,8 @@ int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 
         irq = dispc_mgr_get_vsync_irq(mgr->id);
 
-        mp = get_mgr_priv(mgr);
         i = 0;
         while (1) {
-                unsigned long flags;
                 bool shadow_dirty, dirty;
 
                 spin_lock_irqsave(&data_lock, flags);
@@ -489,21 +492,30 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
 {
         unsigned long timeout = msecs_to_jiffies(500);
         struct ovl_priv_data *op;
-        struct omap_dss_device *dssdev;
+        struct mgr_priv_data *mp;
         u32 irq;
+        unsigned long flags;
         int r;
         int i;
 
         if (!ovl->manager)
                 return 0;
 
-        dssdev = ovl->manager->device;
+        mp = get_mgr_priv(ovl->manager);
+
+        spin_lock_irqsave(&data_lock, flags);
 
-        if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+        if (ovl_manual_update(ovl)) {
+                spin_unlock_irqrestore(&data_lock, flags);
                 return 0;
+        }
 
-        if (ovl_manual_update(ovl))
+        if (!mp->enabled) {
+                spin_unlock_irqrestore(&data_lock, flags);
                 return 0;
+        }
+
+        spin_unlock_irqrestore(&data_lock, flags);
 
         r = dispc_runtime_get();
         if (r)
@@ -514,7 +526,6 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
         op = get_ovl_priv(ovl);
         i = 0;
         while (1) {
-                unsigned long flags;
                 bool shadow_dirty, dirty;
 
                 spin_lock_irqsave(&data_lock, flags);
@@ -573,7 +584,7 @@ static void dss_ovl_write_regs(struct omap_overlay *ovl)
 
         replication = dss_ovl_use_replication(mp->lcd_config, oi->color_mode);
 
-        r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings);
+        r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings, false);
         if (r) {
                 /*
                  * We can't do much here, as this function can be called from
@@ -677,40 +688,11 @@ static void dss_mgr_write_regs_extra(struct omap_overlay_manager *mgr)
                 mp->shadow_extra_info_dirty = true;
 }
 
-static void dss_write_regs_common(void)
-{
-        const int num_mgrs = omap_dss_get_num_overlay_managers();
-        int i;
-
-        if (!dss_data.fifo_merge_dirty)
-                return;
-
-        for (i = 0; i < num_mgrs; ++i) {
-                struct omap_overlay_manager *mgr;
-                struct mgr_priv_data *mp;
-
-                mgr = omap_dss_get_overlay_manager(i);
-                mp = get_mgr_priv(mgr);
-
-                if (mp->enabled) {
-                        if (dss_data.fifo_merge_dirty) {
-                                dispc_enable_fifomerge(dss_data.fifo_merge);
-                                dss_data.fifo_merge_dirty = false;
-                        }
-
-                        if (mp->updating)
-                                mp->shadow_info_dirty = true;
-                }
-        }
-}
-
 static void dss_write_regs(void)
 {
         const int num_mgrs = omap_dss_get_num_overlay_managers();
         int i;
 
-        dss_write_regs_common();
-
         for (i = 0; i < num_mgrs; ++i) {
                 struct omap_overlay_manager *mgr;
                 struct mgr_priv_data *mp;
@@ -799,8 +781,6 @@ void dss_mgr_start_update(struct omap_overlay_manager *mgr)
         dss_mgr_write_regs(mgr);
         dss_mgr_write_regs_extra(mgr);
 
-        dss_write_regs_common();
-
         mp->updating = true;
 
         if (!dss_data.irq_enabled && need_isr())
@@ -984,20 +964,11 @@ static void dss_apply_ovl_fifo_thresholds(struct omap_overlay *ovl,
         op->extra_info_dirty = true;
 }
 
-static void dss_apply_fifo_merge(bool use_fifo_merge)
-{
-        if (dss_data.fifo_merge == use_fifo_merge)
-                return;
-
-        dss_data.fifo_merge = use_fifo_merge;
-        dss_data.fifo_merge_dirty = true;
-}
-
-static void dss_ovl_setup_fifo(struct omap_overlay *ovl,
-                bool use_fifo_merge)
+static void dss_ovl_setup_fifo(struct omap_overlay *ovl)
 {
         struct ovl_priv_data *op = get_ovl_priv(ovl);
         u32 fifo_low, fifo_high;
+        bool use_fifo_merge = false;
 
         if (!op->enabled && !op->enabling)
                 return;
@@ -1008,8 +979,7 @@ static void dss_ovl_setup_fifo(struct omap_overlay *ovl,
         dss_apply_ovl_fifo_thresholds(ovl, fifo_low, fifo_high);
 }
 
-static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr,
-                bool use_fifo_merge)
+static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr)
 {
         struct omap_overlay *ovl;
         struct mgr_priv_data *mp;
@@ -1020,94 +990,19 @@ static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr,
                 return;
 
         list_for_each_entry(ovl, &mgr->overlays, list)
-                dss_ovl_setup_fifo(ovl, use_fifo_merge);
-}
-
-static void dss_setup_fifos(bool use_fifo_merge)
-{
-        const int num_mgrs = omap_dss_get_num_overlay_managers();
-        struct omap_overlay_manager *mgr;
-        int i;
-
-        for (i = 0; i < num_mgrs; ++i) {
-                mgr = omap_dss_get_overlay_manager(i);
-                dss_mgr_setup_fifos(mgr, use_fifo_merge);
-        }
+                dss_ovl_setup_fifo(ovl);
 }
 
-static int get_num_used_managers(void)
+static void dss_setup_fifos(void)
 {
         const int num_mgrs = omap_dss_get_num_overlay_managers();
         struct omap_overlay_manager *mgr;
-        struct mgr_priv_data *mp;
         int i;
-        int enabled_mgrs;
-
-        enabled_mgrs = 0;
 
         for (i = 0; i < num_mgrs; ++i) {
                 mgr = omap_dss_get_overlay_manager(i);
-                mp = get_mgr_priv(mgr);
-
-                if (!mp->enabled)
-                        continue;
-
-                enabled_mgrs++;
+                dss_mgr_setup_fifos(mgr);
         }
-
-        return enabled_mgrs;
-}
-
-static int get_num_used_overlays(void)
-{
-        const int num_ovls = omap_dss_get_num_overlays();
-        struct omap_overlay *ovl;
-        struct ovl_priv_data *op;
-        struct mgr_priv_data *mp;
-        int i;
-        int enabled_ovls;
-
-        enabled_ovls = 0;
-
-        for (i = 0; i < num_ovls; ++i) {
-                ovl = omap_dss_get_overlay(i);
-                op = get_ovl_priv(ovl);
-
-                if (!op->enabled && !op->enabling)
-                        continue;
-
-                mp = get_mgr_priv(ovl->manager);
-
-                if (!mp->enabled)
-                        continue;
-
-                enabled_ovls++;
-        }
-
-        return enabled_ovls;
-}
-
-static bool get_use_fifo_merge(void)
-{
-        int enabled_mgrs = get_num_used_managers();
-        int enabled_ovls = get_num_used_overlays();
-
-        if (!dss_has_feature(FEAT_FIFO_MERGE))
-                return false;
-
-        /*
-         * In theory the only requirement for fifomerge is enabled_ovls <= 1.
-         * However, if we have two managers enabled and set/unset the fifomerge,
-         * we need to set the GO bits in particular sequence for the managers,
-         * and wait in between.
-         *
-         * This is rather difficult as new apply calls can happen at any time,
-         * so we simplify the problem by requiring also that enabled_mgrs <= 1.
-         * In practice this shouldn't matter, because when only one overlay is
-         * enabled, most likely only one output is enabled.
-         */
-
-        return enabled_mgrs <= 1 && enabled_ovls <= 1;
 }
 
 int dss_mgr_enable(struct omap_overlay_manager *mgr)
@@ -1115,7 +1010,6 @@ int dss_mgr_enable(struct omap_overlay_manager *mgr)
         struct mgr_priv_data *mp = get_mgr_priv(mgr);
         unsigned long flags;
         int r;
-        bool fifo_merge;
 
         mutex_lock(&apply_lock);
 
@@ -1133,23 +1027,11 @@ int dss_mgr_enable(struct omap_overlay_manager *mgr)
                 goto err;
         }
 
-        /* step 1: setup fifos/fifomerge before enabling the manager */
-
-        fifo_merge = get_use_fifo_merge();
-        dss_setup_fifos(fifo_merge);
-        dss_apply_fifo_merge(fifo_merge);
+        dss_setup_fifos();
 
         dss_write_regs();
         dss_set_go_bits();
 
-        spin_unlock_irqrestore(&data_lock, flags);
-
-        /* wait until fifo config is in */
-        wait_pending_extra_info_updates();
-
-        /* step 2: enable the manager */
-        spin_lock_irqsave(&data_lock, flags);
-
         if (!mgr_manual_update(mgr))
                 mp->updating = true;
 
@@ -1174,7 +1056,6 @@ void dss_mgr_disable(struct omap_overlay_manager *mgr)
 {
         struct mgr_priv_data *mp = get_mgr_priv(mgr);
         unsigned long flags;
-        bool fifo_merge;
 
         mutex_lock(&apply_lock);
 
@@ -1189,16 +1070,8 @@ void dss_mgr_disable(struct omap_overlay_manager *mgr)
         mp->updating = false;
         mp->enabled = false;
 
-        fifo_merge = get_use_fifo_merge();
-        dss_setup_fifos(fifo_merge);
-        dss_apply_fifo_merge(fifo_merge);
-
-        dss_write_regs();
-        dss_set_go_bits();
-
         spin_unlock_irqrestore(&data_lock, flags);
 
-        wait_pending_extra_info_updates();
 out:
         mutex_unlock(&apply_lock);
 }
@@ -1237,29 +1110,29 @@ void dss_mgr_get_info(struct omap_overlay_manager *mgr,
         spin_unlock_irqrestore(&data_lock, flags);
 }
 
-int dss_mgr_set_device(struct omap_overlay_manager *mgr,
-                struct omap_dss_device *dssdev)
+int dss_mgr_set_output(struct omap_overlay_manager *mgr,
+                struct omap_dss_output *output)
 {
         int r;
 
         mutex_lock(&apply_lock);
 
-        if (dssdev->manager) {
-                DSSERR("display '%s' already has a manager '%s'\n",
-                               dssdev->name, dssdev->manager->name);
+        if (mgr->output) {
+                DSSERR("manager %s is already connected to an output\n",
+                        mgr->name);
                 r = -EINVAL;
                 goto err;
         }
 
-        if ((mgr->supported_displays & dssdev->type) == 0) {
-                DSSERR("display '%s' does not support manager '%s'\n",
-                               dssdev->name, mgr->name);
+        if ((mgr->supported_outputs & output->id) == 0) {
+                DSSERR("output does not support manager %s\n",
+                        mgr->name);
                 r = -EINVAL;
                 goto err;
         }
 
-        dssdev->manager = mgr;
-        mgr->device = dssdev;
+        output->manager = mgr;
+        mgr->output = output;
 
         mutex_unlock(&apply_lock);
 
@@ -1269,40 +1142,46 @@ err:
         return r;
 }
 
-int dss_mgr_unset_device(struct omap_overlay_manager *mgr)
+int dss_mgr_unset_output(struct omap_overlay_manager *mgr)
 {
         int r;
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
 
         mutex_lock(&apply_lock);
 
-        if (!mgr->device) {
-                DSSERR("failed to unset display, display not set.\n");
+        if (!mgr->output) {
+                DSSERR("failed to unset output, output not set\n");
                 r = -EINVAL;
                 goto err;
         }
 
-        /*
-         * Don't allow currently enabled displays to have the overlay manager
-         * pulled out from underneath them
-         */
-        if (mgr->device->state != OMAP_DSS_DISPLAY_DISABLED) {
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (mp->enabled) {
+                DSSERR("output can't be unset when manager is enabled\n");
                 r = -EINVAL;
-                goto err;
+                goto err1;
         }
 
-        mgr->device->manager = NULL;
-        mgr->device = NULL;
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        mgr->output->manager = NULL;
+        mgr->output = NULL;
 
         mutex_unlock(&apply_lock);
 
         return 0;
+err1:
+        spin_unlock_irqrestore(&data_lock, flags);
 err:
         mutex_unlock(&apply_lock);
+
         return r;
 }
 
 static void dss_apply_mgr_timings(struct omap_overlay_manager *mgr,
-                struct omap_video_timings *timings)
+                const struct omap_video_timings *timings)
 {
         struct mgr_priv_data *mp = get_mgr_priv(mgr);
 
@@ -1311,24 +1190,22 @@ static void dss_apply_mgr_timings(struct omap_overlay_manager *mgr,
 }
 
 void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
-                struct omap_video_timings *timings)
+                const struct omap_video_timings *timings)
 {
         unsigned long flags;
-
-        mutex_lock(&apply_lock);
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
 
         spin_lock_irqsave(&data_lock, flags);
 
-        dss_apply_mgr_timings(mgr, timings);
-
-        dss_write_regs();
-        dss_set_go_bits();
+        if (mp->updating) {
+                DSSERR("cannot set timings for %s: manager needs to be disabled\n",
+                        mgr->name);
+                goto out;
+        }
 
+        dss_apply_mgr_timings(mgr, timings);
+out:
         spin_unlock_irqrestore(&data_lock, flags);
-
-        wait_pending_extra_info_updates();
-
-        mutex_unlock(&apply_lock);
 }
 
 static void dss_apply_mgr_lcd_config(struct omap_overlay_manager *mgr,
@@ -1346,7 +1223,7 @@ void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
         unsigned long flags;
         struct mgr_priv_data *mp = get_mgr_priv(mgr);
 
-        mutex_lock(&apply_lock);
+        spin_lock_irqsave(&data_lock, flags);
 
         if (mp->enabled) {
                 DSSERR("cannot apply lcd config for %s: manager needs to be disabled\n",
@@ -1354,19 +1231,9 @@ void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
                 goto out;
         }
 
-        spin_lock_irqsave(&data_lock, flags);
-
         dss_apply_mgr_lcd_config(mgr, config);
-
-        dss_write_regs();
-        dss_set_go_bits();
-
-        spin_unlock_irqrestore(&data_lock, flags);
-
-        wait_pending_extra_info_updates();
-
 out:
-        mutex_unlock(&apply_lock);
+        spin_unlock_irqrestore(&data_lock, flags);
 }
 
 int dss_ovl_set_info(struct omap_overlay *ovl,
@@ -1483,6 +1350,13 @@ int dss_ovl_unset_manager(struct omap_overlay *ovl)
                 goto err;
         }
 
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        /* wait for pending extra_info updates to ensure the ovl is disabled */
+        wait_pending_extra_info_updates();
+
+        spin_lock_irqsave(&data_lock, flags);
+
         op->channel = -1;
 
         ovl->manager = NULL;
@@ -1517,7 +1391,6 @@ int dss_ovl_enable(struct omap_overlay *ovl)
 {
         struct ovl_priv_data *op = get_ovl_priv(ovl);
         unsigned long flags;
-        bool fifo_merge;
         int r;
 
         mutex_lock(&apply_lock);
@@ -1527,7 +1400,7 @@ int dss_ovl_enable(struct omap_overlay *ovl)
                 goto err1;
         }
 
-        if (ovl->manager == NULL || ovl->manager->device == NULL) {
+        if (ovl->manager == NULL || ovl->manager->output == NULL) {
                 r = -EINVAL;
                 goto err1;
         }
@@ -1543,22 +1416,7 @@ int dss_ovl_enable(struct omap_overlay *ovl)
                 goto err2;
         }
 
-        /* step 1: configure fifos/fifomerge for currently enabled ovls */
-
-        fifo_merge = get_use_fifo_merge();
-        dss_setup_fifos(fifo_merge);
-        dss_apply_fifo_merge(fifo_merge);
-
-        dss_write_regs();
-        dss_set_go_bits();
-
-        spin_unlock_irqrestore(&data_lock, flags);
-
-        /* wait for fifo configs to go in */
-        wait_pending_extra_info_updates();
-
-        /* step 2: enable the overlay */
-        spin_lock_irqsave(&data_lock, flags);
+        dss_setup_fifos();
 
         op->enabling = false;
         dss_apply_ovl_enable(ovl, true);
@@ -1568,9 +1426,6 @@ int dss_ovl_enable(struct omap_overlay *ovl)
 
         spin_unlock_irqrestore(&data_lock, flags);
 
-        /* wait for overlay to be enabled */
-        wait_pending_extra_info_updates();
-
         mutex_unlock(&apply_lock);
 
         return 0;
@@ -1586,7 +1441,6 @@ int dss_ovl_disable(struct omap_overlay *ovl)
 {
         struct ovl_priv_data *op = get_ovl_priv(ovl);
         unsigned long flags;
-        bool fifo_merge;
         int r;
 
         mutex_lock(&apply_lock);
@@ -1596,39 +1450,19 @@ int dss_ovl_disable(struct omap_overlay *ovl)
                 goto err;
         }
 
-        if (ovl->manager == NULL || ovl->manager->device == NULL) {
+        if (ovl->manager == NULL || ovl->manager->output == NULL) {
                 r = -EINVAL;
                 goto err;
         }
 
-        /* step 1: disable the overlay */
         spin_lock_irqsave(&data_lock, flags);
 
         dss_apply_ovl_enable(ovl, false);
-
         dss_write_regs();
         dss_set_go_bits();
 
         spin_unlock_irqrestore(&data_lock, flags);
 
-        /* wait for the overlay to be disabled */
-        wait_pending_extra_info_updates();
-
-        /* step 2: configure fifos/fifomerge */
-        spin_lock_irqsave(&data_lock, flags);
-
-        fifo_merge = get_use_fifo_merge();
-        dss_setup_fifos(fifo_merge);
-        dss_apply_fifo_merge(fifo_merge);
-
-        dss_write_regs();
-        dss_set_go_bits();
-
-        spin_unlock_irqrestore(&data_lock, flags);
-
-        /* wait for fifo config to go in */
-        wait_pending_extra_info_updates();
-
         mutex_unlock(&apply_lock);
 
         return 0;
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index 58bd9c27369d..b2af72dc20bd 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -33,6 +33,7 @@
 #include <linux/device.h>
 #include <linux/regulator/consumer.h>
 #include <linux/suspend.h>
+#include <linux/slab.h>
 
 #include <video/omapdss.h>
 
@@ -57,6 +58,11 @@ bool dss_debug;
 module_param_named(debug, dss_debug, bool, 0644);
 #endif
 
+const char *dss_get_default_display_name(void)
+{
+        return core.default_display_name;
+}
+
 /* REGULATORS */
 
 struct regulator *dss_get_vdds_dsi(void)
@@ -347,17 +353,14 @@ static int dss_driver_probe(struct device *dev)
         int r;
         struct omap_dss_driver *dssdrv = to_dss_driver(dev->driver);
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        bool force;
 
         DSSDBG("driver_probe: dev %s/%s, drv %s\n",
                                 dev_name(dev), dssdev->driver_name,
                                 dssdrv->driver.name);
 
-        dss_init_device(core.pdev, dssdev);
-
-        force = core.default_display_name &&
-                strcmp(core.default_display_name, dssdev->name) == 0;
-        dss_recheck_connections(dssdev, force);
+        r = dss_init_device(core.pdev, dssdev);
+        if (r)
+                return r;
 
         r = dssdrv->probe(dssdev);
 
@@ -416,54 +419,44 @@ void omap_dss_unregister_driver(struct omap_dss_driver *dssdriver)
 EXPORT_SYMBOL(omap_dss_unregister_driver);
 
 /* DEVICE */
-static void reset_device(struct device *dev, int check)
-{
-        u8 *dev_p = (u8 *)dev;
-        u8 *dev_end = dev_p + sizeof(*dev);
-        void *saved_pdata;
-
-        saved_pdata = dev->platform_data;
-        if (check) {
-                /*
-                 * Check if there is any other setting than platform_data
-                 * in struct device; warn that these will be reset by our
-                 * init.
-                 */
-                dev->platform_data = NULL;
-                while (dev_p < dev_end) {
-                        if (*dev_p) {
-                                WARN("%s: struct device fields will be "
-                                                "discarded\n",
-                                     __func__);
-                                break;
-                        }
-                        dev_p++;
-                }
-        }
-        memset(dev, 0, sizeof(*dev));
-        dev->platform_data = saved_pdata;
-}
-
 
 static void omap_dss_dev_release(struct device *dev)
 {
-        reset_device(dev, 0);
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        kfree(dssdev);
 }
 
-int omap_dss_register_device(struct omap_dss_device *dssdev,
-                struct device *parent, int disp_num)
+static int disp_num_counter;
+
+struct omap_dss_device *dss_alloc_and_init_device(struct device *parent)
 {
-        WARN_ON(!dssdev->driver_name);
+        struct omap_dss_device *dssdev;
+
+        dssdev = kzalloc(sizeof(*dssdev), GFP_KERNEL);
+        if (!dssdev)
+                return NULL;
 
-        reset_device(&dssdev->dev, 1);
         dssdev->dev.bus = &dss_bus_type;
         dssdev->dev.parent = parent;
         dssdev->dev.release = omap_dss_dev_release;
-        dev_set_name(&dssdev->dev, "display%d", disp_num);
-        return device_register(&dssdev->dev);
+        dev_set_name(&dssdev->dev, "display%d", disp_num_counter++);
+
+        device_initialize(&dssdev->dev);
+
+        return dssdev;
+}
+
+int dss_add_device(struct omap_dss_device *dssdev)
+{
+        return device_add(&dssdev->dev);
+}
+
+void dss_put_device(struct omap_dss_device *dssdev)
+{
+        put_device(&dssdev->dev);
 }
 
-void omap_dss_unregister_device(struct omap_dss_device *dssdev)
+void dss_unregister_device(struct omap_dss_device *dssdev)
 {
         device_unregister(&dssdev->dev);
 }
@@ -471,15 +464,25 @@ void omap_dss_unregister_device(struct omap_dss_device *dssdev)
 static int dss_unregister_dss_dev(struct device *dev, void *data)
 {
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        omap_dss_unregister_device(dssdev);
+        dss_unregister_device(dssdev);
         return 0;
 }
 
-void omap_dss_unregister_child_devices(struct device *parent)
+void dss_unregister_child_devices(struct device *parent)
 {
         device_for_each_child(parent, NULL, dss_unregister_dss_dev);
 }
 
+void dss_copy_device_pdata(struct omap_dss_device *dst,
+                const struct omap_dss_device *src)
+{
+        u8 *d = (u8 *)dst;
+        u8 *s = (u8 *)src;
+        size_t dsize = sizeof(struct device);
+
+        memcpy(d + dsize, s + dsize, sizeof(struct omap_dss_device) - dsize);
+}
+
 /* BUS */
 static int __init omap_dss_bus_register(void)
 {
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index ee9e29639dcc..b43477a5fae8 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -38,7 +38,6 @@
 #include <linux/pm_runtime.h>
 
 #include <plat/cpu.h>
-#include <plat/clock.h>
 
 #include <video/omapdss.h>
 
@@ -82,6 +81,30 @@ struct dispc_irq_stats {
         unsigned irqs[32];
 };
 
+struct dispc_features {
+        u8 sw_start;
+        u8 fp_start;
+        u8 bp_start;
+        u16 sw_max;
+        u16 vp_max;
+        u16 hp_max;
+        int (*calc_scaling) (enum omap_plane plane,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem);
+        unsigned long (*calc_core_clk) (enum omap_plane plane,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                bool mem_to_mem);
+        u8 num_fifos;
+
+        /* swap GFX & WB fifos */
+        bool gfx_fifo_workaround:1;
+};
+
+#define DISPC_MAX_NR_FIFOS 5
+
 static struct {
         struct platform_device *pdev;
         void __iomem    *base;
@@ -91,7 +114,9 @@ static struct {
         int irq;
         struct clk *dss_clk;
 
-        u32     fifo_size[MAX_DSS_OVERLAYS];
+        u32 fifo_size[DISPC_MAX_NR_FIFOS];
+        /* maps which plane is using a fifo. fifo-id -> plane-id */
+        int fifo_assignment[DISPC_MAX_NR_FIFOS];
 
         spinlock_t irq_lock;
         u32 irq_error_mask;
@@ -102,6 +127,8 @@ static struct {
         bool            ctx_valid;
         u32             ctx[DISPC_SZ_REGS / sizeof(u32)];
 
+        const struct dispc_features *feat;
+
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
         spinlock_t irq_stats_lock;
         struct dispc_irq_stats irq_stats;
@@ -211,7 +238,14 @@ static const struct {
         },
 };
 
+struct color_conv_coef {
+        int ry, rcr, rcb, gy, gcr, gcb, by, bcr, bcb;
+        int full_range;
+};
+
 static void _omap_dispc_set_irqs(void);
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane);
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane);
 
 static inline void dispc_write_reg(const u16 idx, u32 val)
 {
@@ -509,6 +543,11 @@ u32 dispc_mgr_get_framedone_irq(enum omap_channel channel)
         return mgr_desc[channel].framedone_irq;
 }
 
+u32 dispc_wb_get_framedone_irq(void)
+{
+        return DISPC_IRQ_FRAMEDONEWB;
+}
+
 bool dispc_mgr_go_busy(enum omap_channel channel)
 {
         return mgr_fld_read(channel, DISPC_MGR_FLD_GO) == 1;
@@ -536,6 +575,30 @@ void dispc_mgr_go(enum omap_channel channel)
         mgr_fld_write(channel, DISPC_MGR_FLD_GO, 1);
 }
 
+bool dispc_wb_go_busy(void)
+{
+        return REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+}
+
+void dispc_wb_go(void)
+{
+        enum omap_plane plane = OMAP_DSS_WB;
+        bool enable, go;
+
+        enable = REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0) == 1;
+
+        if (!enable)
+                return;
+
+        go = REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+        if (go) {
+                DSSERR("GO bit not down for WB\n");
+                return;
+        }
+
+        REG_FLD_MOD(DISPC_CONTROL2, 1, 6, 6);
+}
+
 static void dispc_ovl_write_firh_reg(enum omap_plane plane, int reg, u32 value)
 {
         dispc_write_reg(DISPC_OVL_FIR_COEF_H(plane, reg), value);
@@ -618,41 +681,41 @@ static void dispc_ovl_set_scale_coef(enum omap_plane plane, int fir_hinc,
         }
 }
 
-static void _dispc_setup_color_conv_coef(void)
-{
-        int i;
-        const struct color_conv_coef {
-                int  ry,  rcr,  rcb,   gy,  gcr,  gcb,   by,  bcr,  bcb;
-                int  full_range;
-        }  ctbl_bt601_5 = {
-                298,  409,    0,  298, -208, -100,  298,    0,  517, 0,
-        };
-
-        const struct color_conv_coef *ct;
 
+static void dispc_ovl_write_color_conv_coef(enum omap_plane plane,
+                const struct color_conv_coef *ct)
+{
 #define CVAL(x, y) (FLD_VAL(x, 26, 16) | FLD_VAL(y, 10, 0))
 
-        ct = &ctbl_bt601_5;
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 0), CVAL(ct->rcr, ct->ry));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 1), CVAL(ct->gy,  ct->rcb));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 2), CVAL(ct->gcb, ct->gcr));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 3), CVAL(ct->bcr, ct->by));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 4), CVAL(0, ct->bcb));
 
-        for (i = 1; i < dss_feat_get_num_ovls(); i++) {
-                dispc_write_reg(DISPC_OVL_CONV_COEF(i, 0),
-                        CVAL(ct->rcr, ct->ry));
-                dispc_write_reg(DISPC_OVL_CONV_COEF(i, 1),
-                        CVAL(ct->gy,  ct->rcb));
-                dispc_write_reg(DISPC_OVL_CONV_COEF(i, 2),
-                        CVAL(ct->gcb, ct->gcr));
-                dispc_write_reg(DISPC_OVL_CONV_COEF(i, 3),
-                        CVAL(ct->bcr, ct->by));
-                dispc_write_reg(DISPC_OVL_CONV_COEF(i, 4),
-                        CVAL(0, ct->bcb));
-
-                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(i), ct->full_range,
-                        11, 11);
-        }
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), ct->full_range, 11, 11);
 
 #undef CVAL
 }
 
+static void dispc_setup_color_conv_coef(void)
+{
+        int i;
+        int num_ovl = dss_feat_get_num_ovls();
+        int num_wb = dss_feat_get_num_wbs();
+        const struct color_conv_coef ctbl_bt601_5_ovl = {
+                298, 409, 0, 298, -208, -100, 298, 0, 517, 0,
+        };
+        const struct color_conv_coef ctbl_bt601_5_wb = {
+                66, 112, -38, 129, -94, -74, 25, -18, 112, 0,
+        };
+
+        for (i = 1; i < num_ovl; i++)
+                dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_ovl);
+
+        for (; i < num_wb; i++)
+                dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_wb);
+}
 
 static void dispc_ovl_set_ba0(enum omap_plane plane, u32 paddr)
 {
@@ -674,24 +737,32 @@ static void dispc_ovl_set_ba1_uv(enum omap_plane plane, u32 paddr)
         dispc_write_reg(DISPC_OVL_BA1_UV(plane), paddr);
 }
 
-static void dispc_ovl_set_pos(enum omap_plane plane, int x, int y)
+static void dispc_ovl_set_pos(enum omap_plane plane,
+                enum omap_overlay_caps caps, int x, int y)
 {
-        u32 val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
+        u32 val;
+
+        if ((caps & OMAP_DSS_OVL_CAP_POS) == 0)
+                return;
+
+        val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
 
         dispc_write_reg(DISPC_OVL_POSITION(plane), val);
 }
 
-static void dispc_ovl_set_pic_size(enum omap_plane plane, int width, int height)
+static void dispc_ovl_set_input_size(enum omap_plane plane, int width,
+                int height)
 {
         u32 val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
 
-        if (plane == OMAP_DSS_GFX)
+        if (plane == OMAP_DSS_GFX || plane == OMAP_DSS_WB)
                 dispc_write_reg(DISPC_OVL_SIZE(plane), val);
         else
                 dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
 }
 
-static void dispc_ovl_set_vid_size(enum omap_plane plane, int width, int height)
+static void dispc_ovl_set_output_size(enum omap_plane plane, int width,
+                int height)
 {
         u32 val;
 
@@ -699,14 +770,16 @@ static void dispc_ovl_set_vid_size(enum omap_plane plane, int width, int height)
 
         val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
 
-        dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+        if (plane == OMAP_DSS_WB)
+                dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
+        else
+                dispc_write_reg(DISPC_OVL_SIZE(plane), val);
 }
 
-static void dispc_ovl_set_zorder(enum omap_plane plane, u8 zorder)
+static void dispc_ovl_set_zorder(enum omap_plane plane,
+                enum omap_overlay_caps caps, u8 zorder)
 {
-        struct omap_overlay *ovl = omap_dss_get_overlay(plane);
-
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+        if ((caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
                 return;
 
         REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), zorder, 27, 26);
@@ -723,23 +796,22 @@ static void dispc_ovl_enable_zorder_planes(void)
                 REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(i), 1, 25, 25);
 }
 
-static void dispc_ovl_set_pre_mult_alpha(enum omap_plane plane, bool enable)
+static void dispc_ovl_set_pre_mult_alpha(enum omap_plane plane,
+                enum omap_overlay_caps caps, bool enable)
 {
-        struct omap_overlay *ovl = omap_dss_get_overlay(plane);
-
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+        if ((caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
                 return;
 
         REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 28, 28);
 }
 
-static void dispc_ovl_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
+static void dispc_ovl_setup_global_alpha(enum omap_plane plane,
+                enum omap_overlay_caps caps, u8 global_alpha)
 {
         static const unsigned shifts[] = { 0, 8, 16, 24, };
         int shift;
-        struct omap_overlay *ovl = omap_dss_get_overlay(plane);
 
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+        if ((caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
                 return;
 
         shift = shifts[plane];
@@ -947,10 +1019,17 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
         return channel;
 }
 
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel)
+{
+        enum omap_plane plane = OMAP_DSS_WB;
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), channel, 18, 16);
+}
+
 static void dispc_ovl_set_burst_size(enum omap_plane plane,
                 enum omap_burst_size burst_size)
 {
-        static const unsigned shifts[] = { 6, 14, 14, 14, };
+        static const unsigned shifts[] = { 6, 14, 14, 14, 14, };
         int shift;
 
         shift = shifts[plane];
@@ -1027,11 +1106,15 @@ static void dispc_ovl_set_vid_color_conv(enum omap_plane plane, bool enable)
         dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 }
 
-static void dispc_ovl_enable_replication(enum omap_plane plane, bool enable)
+static void dispc_ovl_enable_replication(enum omap_plane plane,
+                enum omap_overlay_caps caps, bool enable)
 {
         static const unsigned shifts[] = { 5, 10, 10, 10 };
         int shift;
 
+        if ((caps & OMAP_DSS_OVL_CAP_REPLICATION) == 0)
+                return;
+
         shift = shifts[plane];
         REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, shift, shift);
 }
@@ -1045,10 +1128,10 @@ static void dispc_mgr_set_size(enum omap_channel channel, u16 width,
         dispc_write_reg(DISPC_SIZE_MGR(channel), val);
 }
 
-static void dispc_read_plane_fifo_sizes(void)
+static void dispc_init_fifos(void)
 {
         u32 size;
-        int plane;
+        int fifo;
         u8 start, end;
         u32 unit;
 
@@ -1056,16 +1139,53 @@ static void dispc_read_plane_fifo_sizes(void)
 
         dss_feat_get_reg_field(FEAT_REG_FIFOSIZE, &start, &end);
 
-        for (plane = 0; plane < dss_feat_get_num_ovls(); ++plane) {
-                size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(plane), start, end);
+        for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+                size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(fifo), start, end);
                 size *= unit;
-                dispc.fifo_size[plane] = size;
+                dispc.fifo_size[fifo] = size;
+
+                /*
+                 * By default fifos are mapped directly to overlays, fifo 0 to
+                 * ovl 0, fifo 1 to ovl 1, etc.
+                 */
+                dispc.fifo_assignment[fifo] = fifo;
+        }
+
+        /*
+         * The GFX fifo on OMAP4 is smaller than the other fifos. The small fifo
+         * causes problems with certain use cases, like using the tiler in 2D
+         * mode. The below hack swaps the fifos of GFX and WB planes, thus
+         * giving GFX plane a larger fifo. WB but should work fine with a
+         * smaller fifo.
+         */
+        if (dispc.feat->gfx_fifo_workaround) {
+                u32 v;
+
+                v = dispc_read_reg(DISPC_GLOBAL_BUFFER);
+
+                v = FLD_MOD(v, 4, 2, 0); /* GFX BUF top to WB */
+                v = FLD_MOD(v, 4, 5, 3); /* GFX BUF bottom to WB */
+                v = FLD_MOD(v, 0, 26, 24); /* WB BUF top to GFX */
+                v = FLD_MOD(v, 0, 29, 27); /* WB BUF bottom to GFX */
+
+                dispc_write_reg(DISPC_GLOBAL_BUFFER, v);
+
+                dispc.fifo_assignment[OMAP_DSS_GFX] = OMAP_DSS_WB;
+                dispc.fifo_assignment[OMAP_DSS_WB] = OMAP_DSS_GFX;
         }
 }
 
 static u32 dispc_ovl_get_fifo_size(enum omap_plane plane)
 {
-        return dispc.fifo_size[plane];
+        int fifo;
+        u32 size = 0;
+
+        for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+                if (dispc.fifo_assignment[fifo] == plane)
+                        size += dispc.fifo_size[fifo];
+        }
+
+        return size;
 }
 
 void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
@@ -1141,6 +1261,14 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
         if (manual_update && dss_has_feature(FEAT_OMAP3_DSI_FIFO_BUG)) {
                 *fifo_low = ovl_fifo_size - burst_size * 2;
                 *fifo_high = total_fifo_size - burst_size;
+        } else if (plane == OMAP_DSS_WB) {
+                /*
+                 * Most optimal configuration for writeback is to push out data
+                 * to the interconnect the moment writeback pushes enough pixels
+                 * in the FIFO to form a burst
+                 */
+                *fifo_low = 0;
+                *fifo_high = burst_size;
         } else {
                 *fifo_low = ovl_fifo_size - burst_size;
                 *fifo_high = total_fifo_size - buf_unit;
@@ -1383,6 +1511,7 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
 {
         int scale_x = out_width != orig_width;
         int scale_y = out_height != orig_height;
+        bool chroma_upscale = plane != OMAP_DSS_WB ? true : false;
 
         if (!dss_has_feature(FEAT_HANDLE_UV_SEPARATE))
                 return;
@@ -1390,7 +1519,8 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
                         color_mode != OMAP_DSS_COLOR_UYVY &&
                         color_mode != OMAP_DSS_COLOR_NV12)) {
                 /* reset chroma resampling for RGB formats  */
-                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
+                if (plane != OMAP_DSS_WB)
+                        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
                 return;
         }
 
@@ -1399,23 +1529,34 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
 
         switch (color_mode) {
         case OMAP_DSS_COLOR_NV12:
-                /* UV is subsampled by 2 vertically*/
-                orig_height >>= 1;
-                /* UV is subsampled by 2 horz.*/
-                orig_width >>= 1;
+                if (chroma_upscale) {
+                        /* UV is subsampled by 2 horizontally and vertically */
+                        orig_height >>= 1;
+                        orig_width >>= 1;
+                } else {
+                        /* UV is downsampled by 2 horizontally and vertically */
+                        orig_height <<= 1;
+                        orig_width <<= 1;
+                }
+
                 break;
         case OMAP_DSS_COLOR_YUV2:
         case OMAP_DSS_COLOR_UYVY:
-                /*For YUV422 with 90/270 rotation,
-                 *we don't upsample chroma
-                 */
+                /* For YUV422 with 90/270 rotation, we don't upsample chroma */
                 if (rotation == OMAP_DSS_ROT_0 ||
-                        rotation == OMAP_DSS_ROT_180)
-                        /* UV is subsampled by 2 hrz*/
-                        orig_width >>= 1;
+                                rotation == OMAP_DSS_ROT_180) {
+                        if (chroma_upscale)
+                                /* UV is subsampled by 2 horizontally */
+                                orig_width >>= 1;
+                        else
+                                /* UV is downsampled by 2 horizontally */
+                                orig_width <<= 1;
+                }
+
                 /* must use FIR for YUV422 if rotated */
                 if (rotation != OMAP_DSS_ROT_0)
                         scale_x = scale_y = true;
+
                 break;
         default:
                 BUG();
@@ -1431,8 +1572,10 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
                         out_width, out_height, five_taps,
                                 rotation, DISPC_COLOR_COMPONENT_UV);
 
-        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
-                (scale_x || scale_y) ? 1 : 0, 8, 8);
+        if (plane != OMAP_DSS_WB)
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
+                        (scale_x || scale_y) ? 1 : 0, 8, 8);
+
         /* set H scaling */
         REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_x ? 1 : 0, 5, 5);
         /* set V scaling */
@@ -1848,22 +1991,19 @@ static void calc_tiler_rotation_offset(u16 screen_width, u16 width,
  * This function is used to avoid synclosts in OMAP3, because of some
  * undocumented horizontal position and timing related limitations.
  */
-static int check_horiz_timing_omap3(enum omap_channel channel,
+static int check_horiz_timing_omap3(enum omap_plane plane,
                 const struct omap_video_timings *t, u16 pos_x,
                 u16 width, u16 height, u16 out_width, u16 out_height)
 {
         int DS = DIV_ROUND_UP(height, out_height);
-        unsigned long nonactive, lclk, pclk;
+        unsigned long nonactive;
         static const u8 limits[3] = { 8, 10, 20 };
         u64 val, blank;
+        unsigned long pclk = dispc_plane_pclk_rate(plane);
+        unsigned long lclk = dispc_plane_lclk_rate(plane);
         int i;
 
         nonactive = t->x_res + t->hfp + t->hsw + t->hbp - out_width;
-        pclk = dispc_mgr_pclk_rate(channel);
-        if (dss_mgr_is_lcd(channel))
-                lclk = dispc_mgr_lclk_rate(channel);
-        else
-                lclk = dispc_fclk_rate();
 
         i = 0;
         if (out_height < height)
@@ -1900,13 +2040,14 @@ static int check_horiz_timing_omap3(enum omap_channel channel,
         return 0;
 }
 
-static unsigned long calc_core_clk_five_taps(enum omap_channel channel,
+static unsigned long calc_core_clk_five_taps(enum omap_plane plane,
                 const struct omap_video_timings *mgr_timings, u16 width,
                 u16 height, u16 out_width, u16 out_height,
                 enum omap_color_mode color_mode)
 {
         u32 core_clk = 0;
-        u64 tmp, pclk = dispc_mgr_pclk_rate(channel);
+        u64 tmp;
+        unsigned long pclk = dispc_plane_pclk_rate(plane);
 
         if (height <= out_height && width <= out_width)
                 return (unsigned long) pclk;
@@ -1940,11 +2081,22 @@ static unsigned long calc_core_clk_five_taps(enum omap_channel channel,
         return core_clk;
 }
 
-static unsigned long calc_core_clk(enum omap_channel channel, u16 width,
-                u16 height, u16 out_width, u16 out_height)
+static unsigned long calc_core_clk_24xx(enum omap_plane plane, u16 width,
+                u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+        unsigned long pclk = dispc_plane_pclk_rate(plane);
+
+        if (height > out_height && width > out_width)
+                return pclk * 4;
+        else
+                return pclk * 2;
+}
+
+static unsigned long calc_core_clk_34xx(enum omap_plane plane, u16 width,
+                u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
 {
         unsigned int hf, vf;
-        unsigned long pclk = dispc_mgr_pclk_rate(channel);
+        unsigned long pclk = dispc_plane_pclk_rate(plane);
 
         /*
          * FIXME how to determine the 'A' factor
@@ -1959,51 +2111,207 @@ static unsigned long calc_core_clk(enum omap_channel channel, u16 width,
                 hf = 2;
         else
                 hf = 1;
-
         if (height > out_height)
                 vf = 2;
         else
                 vf = 1;
 
-        if (cpu_is_omap24xx()) {
-                if (vf > 1 && hf > 1)
-                        return pclk * 4;
-                else
-                        return pclk * 2;
-        } else if (cpu_is_omap34xx()) {
-                return pclk * vf * hf;
-        } else {
-                if (hf > 1)
-                        return DIV_ROUND_UP(pclk, out_width) * width;
-                else
-                        return pclk;
+        return pclk * vf * hf;
+}
+
+static unsigned long calc_core_clk_44xx(enum omap_plane plane, u16 width,
+                u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+        unsigned long pclk;
+
+        /*
+         * If the overlay/writeback is in mem to mem mode, there are no
+         * downscaling limitations with respect to pixel clock, return 1 as
+         * required core clock to represent that we have sufficient enough
+         * core clock to do maximum downscaling
+         */
+        if (mem_to_mem)
+                return 1;
+
+        pclk = dispc_plane_pclk_rate(plane);
+
+        if (width > out_width)
+                return DIV_ROUND_UP(pclk, out_width) * width;
+        else
+                return pclk;
+}
+
+static int dispc_ovl_calc_scaling_24xx(enum omap_plane plane,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+        int error;
+        u16 in_width, in_height;
+        int min_factor = min(*decim_x, *decim_y);
+        const int maxsinglelinewidth =
+                        dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+        *five_taps = false;
+
+        do {
+                in_height = DIV_ROUND_UP(height, *decim_y);
+                in_width = DIV_ROUND_UP(width, *decim_x);
+                *core_clk = dispc.feat->calc_core_clk(plane, in_width,
+                                in_height, out_width, out_height, mem_to_mem);
+                error = (in_width > maxsinglelinewidth || !*core_clk ||
+                        *core_clk > dispc_core_clk_rate());
+                if (error) {
+                        if (*decim_x == *decim_y) {
+                                *decim_x = min_factor;
+                                ++*decim_y;
+                        } else {
+                                swap(*decim_x, *decim_y);
+                                if (*decim_x < *decim_y)
+                                        ++*decim_x;
+                        }
+                }
+        } while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+        if (in_width > maxsinglelinewidth) {
+                DSSERR("Cannot scale max input width exceeded");
+                return -EINVAL;
         }
+        return 0;
 }
 
-static int dispc_ovl_calc_scaling(enum omap_plane plane,
-                enum omap_channel channel,
+static int dispc_ovl_calc_scaling_34xx(enum omap_plane plane,
                 const struct omap_video_timings *mgr_timings,
                 u16 width, u16 height, u16 out_width, u16 out_height,
                 enum omap_color_mode color_mode, bool *five_taps,
-                int *x_predecim, int *y_predecim, u16 pos_x)
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
 {
-        struct omap_overlay *ovl = omap_dss_get_overlay(plane);
-        const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+        int error;
+        u16 in_width, in_height;
+        int min_factor = min(*decim_x, *decim_y);
+        const int maxsinglelinewidth =
+                        dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+        do {
+                in_height = DIV_ROUND_UP(height, *decim_y);
+                in_width = DIV_ROUND_UP(width, *decim_x);
+                *core_clk = calc_core_clk_five_taps(plane, mgr_timings,
+                        in_width, in_height, out_width, out_height, color_mode);
+
+                error = check_horiz_timing_omap3(plane, mgr_timings,
+                                pos_x, in_width, in_height, out_width,
+                                out_height);
+
+                if (in_width > maxsinglelinewidth)
+                        if (in_height > out_height &&
+                                                in_height < out_height * 2)
+                                *five_taps = false;
+                if (!*five_taps)
+                        *core_clk = dispc.feat->calc_core_clk(plane, in_width,
+                                        in_height, out_width, out_height,
+                                        mem_to_mem);
+
+                error = (error || in_width > maxsinglelinewidth * 2 ||
+                        (in_width > maxsinglelinewidth && *five_taps) ||
+                        !*core_clk || *core_clk > dispc_core_clk_rate());
+                if (error) {
+                        if (*decim_x == *decim_y) {
+                                *decim_x = min_factor;
+                                ++*decim_y;
+                        } else {
+                                swap(*decim_x, *decim_y);
+                                if (*decim_x < *decim_y)
+                                        ++*decim_x;
+                        }
+                }
+        } while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+        if (check_horiz_timing_omap3(plane, mgr_timings, pos_x, width, height,
+                out_width, out_height)){
+                        DSSERR("horizontal timing too tight\n");
+                        return -EINVAL;
+        }
+
+        if (in_width > (maxsinglelinewidth * 2)) {
+                DSSERR("Cannot setup scaling");
+                DSSERR("width exceeds maximum width possible");
+                return -EINVAL;
+        }
+
+        if (in_width > maxsinglelinewidth && *five_taps) {
+                DSSERR("cannot setup scaling with five taps");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int dispc_ovl_calc_scaling_44xx(enum omap_plane plane,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+        u16 in_width, in_width_max;
+        int decim_x_min = *decim_x;
+        u16 in_height = DIV_ROUND_UP(height, *decim_y);
         const int maxsinglelinewidth =
                                 dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+        unsigned long pclk = dispc_plane_pclk_rate(plane);
+        const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+
+        if (mem_to_mem)
+                in_width_max = DIV_ROUND_UP(out_width, maxdownscale);
+        else
+                in_width_max = dispc_core_clk_rate() /
+                                        DIV_ROUND_UP(pclk, out_width);
+
+        *decim_x = DIV_ROUND_UP(width, in_width_max);
+
+        *decim_x = *decim_x > decim_x_min ? *decim_x : decim_x_min;
+        if (*decim_x > *x_predecim)
+                return -EINVAL;
+
+        do {
+                in_width = DIV_ROUND_UP(width, *decim_x);
+        } while (*decim_x <= *x_predecim &&
+                        in_width > maxsinglelinewidth && ++*decim_x);
+
+        if (in_width > maxsinglelinewidth) {
+                DSSERR("Cannot scale width exceeds max line width");
+                return -EINVAL;
+        }
+
+        *core_clk = dispc.feat->calc_core_clk(plane, in_width, in_height,
+                                out_width, out_height, mem_to_mem);
+        return 0;
+}
+
+static int dispc_ovl_calc_scaling(enum omap_plane plane,
+                enum omap_overlay_caps caps,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, u16 pos_x,
+                enum omap_dss_rotation_type rotation_type, bool mem_to_mem)
+{
+        const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
         const int max_decim_limit = 16;
         unsigned long core_clk = 0;
-        int decim_x, decim_y, error, min_factor;
-        u16 in_width, in_height, in_width_max = 0;
+        int decim_x, decim_y, ret;
 
         if (width == out_width && height == out_height)
                 return 0;
 
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0)
+        if ((caps & OMAP_DSS_OVL_CAP_SCALE) == 0)
                 return -EINVAL;
 
         *x_predecim = max_decim_limit;
-        *y_predecim = max_decim_limit;
+        *y_predecim = (rotation_type == OMAP_DSS_ROT_TILER &&
+                        dss_has_feature(FEAT_BURST_2D)) ? 2 : max_decim_limit;
 
         if (color_mode == OMAP_DSS_COLOR_CLUT1 ||
             color_mode == OMAP_DSS_COLOR_CLUT2 ||
@@ -2018,118 +2326,18 @@ static int dispc_ovl_calc_scaling(enum omap_plane plane,
         decim_x = DIV_ROUND_UP(DIV_ROUND_UP(width, out_width), maxdownscale);
         decim_y = DIV_ROUND_UP(DIV_ROUND_UP(height, out_height), maxdownscale);
 
-        min_factor = min(decim_x, decim_y);
-
         if (decim_x > *x_predecim || out_width > width * 8)
                 return -EINVAL;
 
         if (decim_y > *y_predecim || out_height > height * 8)
                 return -EINVAL;
 
-        if (cpu_is_omap24xx()) {
-                *five_taps = false;
-
-                do {
-                        in_height = DIV_ROUND_UP(height, decim_y);
-                        in_width = DIV_ROUND_UP(width, decim_x);
-                        core_clk = calc_core_clk(channel, in_width, in_height,
-                                        out_width, out_height);
-                        error = (in_width > maxsinglelinewidth || !core_clk ||
-                                core_clk > dispc_core_clk_rate());
-                        if (error) {
-                                if (decim_x == decim_y) {
-                                        decim_x = min_factor;
-                                        decim_y++;
-                                } else {
-                                        swap(decim_x, decim_y);
-                                        if (decim_x < decim_y)
-                                                decim_x++;
-                                }
-                        }
-                } while (decim_x <= *x_predecim && decim_y <= *y_predecim &&
-                                error);
-
-                if (in_width > maxsinglelinewidth) {
-                        DSSERR("Cannot scale max input width exceeded");
-                        return -EINVAL;
-                }
-        } else if (cpu_is_omap34xx()) {
-
-                do {
-                        in_height = DIV_ROUND_UP(height, decim_y);
-                        in_width = DIV_ROUND_UP(width, decim_x);
-                        core_clk = calc_core_clk_five_taps(channel, mgr_timings,
-                                in_width, in_height, out_width, out_height,
-                                color_mode);
-
-                        error = check_horiz_timing_omap3(channel, mgr_timings,
-                                pos_x, in_width, in_height, out_width,
-                                out_height);
-
-                        if (in_width > maxsinglelinewidth)
-                                if (in_height > out_height &&
-                                        in_height < out_height * 2)
-                                        *five_taps = false;
-                        if (!*five_taps)
-                                core_clk = calc_core_clk(channel, in_width,
-                                        in_height, out_width, out_height);
-                        error = (error || in_width > maxsinglelinewidth * 2 ||
-                                (in_width > maxsinglelinewidth && *five_taps) ||
-                                !core_clk || core_clk > dispc_core_clk_rate());
-                        if (error) {
-                                if (decim_x == decim_y) {
-                                        decim_x = min_factor;
-                                        decim_y++;
-                                } else {
-                                        swap(decim_x, decim_y);
-                                        if (decim_x < decim_y)
-                                                decim_x++;
-                                }
-                        }
-                } while (decim_x <= *x_predecim && decim_y <= *y_predecim
-                        && error);
-
-                if (check_horiz_timing_omap3(channel, mgr_timings, pos_x, width,
-                        height, out_width, out_height)){
-                                DSSERR("horizontal timing too tight\n");
-                                return -EINVAL;
-                }
-
-                if (in_width > (maxsinglelinewidth * 2)) {
-                        DSSERR("Cannot setup scaling");
-                        DSSERR("width exceeds maximum width possible");
-                        return -EINVAL;
-                }
-
-                if (in_width > maxsinglelinewidth && *five_taps) {
-                        DSSERR("cannot setup scaling with five taps");
-                        return -EINVAL;
-                }
-        } else {
-                int decim_x_min = decim_x;
-                in_height = DIV_ROUND_UP(height, decim_y);
-                in_width_max = dispc_core_clk_rate() /
-                                DIV_ROUND_UP(dispc_mgr_pclk_rate(channel),
-                                                out_width);
-                decim_x = DIV_ROUND_UP(width, in_width_max);
-
-                decim_x = decim_x > decim_x_min ? decim_x : decim_x_min;
-                if (decim_x > *x_predecim)
-                        return -EINVAL;
-
-                do {
-                        in_width = DIV_ROUND_UP(width, decim_x);
-                } while (decim_x <= *x_predecim &&
-                                in_width > maxsinglelinewidth && decim_x++);
-
-                if (in_width > maxsinglelinewidth) {
-                        DSSERR("Cannot scale width exceeds max line width");
-                        return -EINVAL;
-                }
-
-                core_clk = calc_core_clk(channel, in_width, in_height,
-                                out_width, out_height);
-        }
+        ret = dispc.feat->calc_scaling(plane, mgr_timings, width, height,
+                out_width, out_height, color_mode, five_taps,
+                x_predecim, y_predecim, &decim_x, &decim_y, pos_x, &core_clk,
+                mem_to_mem);
+        if (ret)
+                return ret;
 
         DSSDBG("required core clk rate = %lu Hz\n", core_clk);
         DSSDBG("current core clk rate = %lu Hz\n", dispc_core_clk_rate());
@@ -2147,69 +2355,64 @@ static int dispc_ovl_calc_scaling(enum omap_plane plane,
         return 0;
 }
 
-int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
-                bool replication, const struct omap_video_timings *mgr_timings)
+static int dispc_ovl_setup_common(enum omap_plane plane,
+                enum omap_overlay_caps caps, u32 paddr, u32 p_uv_addr,
+                u16 screen_width, int pos_x, int pos_y, u16 width, u16 height,
+                u16 out_width, u16 out_height, enum omap_color_mode color_mode,
+                u8 rotation, bool mirror, u8 zorder, u8 pre_mult_alpha,
+                u8 global_alpha, enum omap_dss_rotation_type rotation_type,
+                bool replication, const struct omap_video_timings *mgr_timings,
+                bool mem_to_mem)
 {
-        struct omap_overlay *ovl = omap_dss_get_overlay(plane);
         bool five_taps = true;
         bool fieldmode = 0;
         int r, cconv = 0;
         unsigned offset0, offset1;
         s32 row_inc;
         s32 pix_inc;
-        u16 frame_height = oi->height;
+        u16 frame_height = height;
         unsigned int field_offset = 0;
-        u16 in_height = oi->height;
-        u16 in_width = oi->width;
-        u16 out_width, out_height;
-        enum omap_channel channel;
+        u16 in_height = height;
+        u16 in_width = width;
         int x_predecim = 1, y_predecim = 1;
         bool ilace = mgr_timings->interlace;
 
-        channel = dispc_ovl_get_channel_out(plane);
-
-        DSSDBG("dispc_ovl_setup %d, pa %x, pa_uv %x, sw %d, %d,%d, %dx%d -> "
-                "%dx%d, cmode %x, rot %d, mir %d, ilace %d chan %d repl %d\n",
-                plane, oi->paddr, oi->p_uv_addr,
-                oi->screen_width, oi->pos_x, oi->pos_y, oi->width, oi->height,
-                oi->out_width, oi->out_height, oi->color_mode, oi->rotation,
-                oi->mirror, ilace, channel, replication);
-
-        if (oi->paddr == 0)
+        if (paddr == 0)
                 return -EINVAL;
 
-        out_width = oi->out_width == 0 ? oi->width : oi->out_width;
-        out_height = oi->out_height == 0 ? oi->height : oi->out_height;
+        out_width = out_width == 0 ? width : out_width;
+        out_height = out_height == 0 ? height : out_height;
 
-        if (ilace && oi->height == out_height)
+        if (ilace && height == out_height)
                 fieldmode = 1;
 
         if (ilace) {
                 if (fieldmode)
                         in_height /= 2;
-                oi->pos_y /= 2;
+                pos_y /= 2;
                 out_height /= 2;
 
                 DSSDBG("adjusting for ilace: height %d, pos_y %d, "
-                                "out_height %d\n",
-                                in_height, oi->pos_y, out_height);
+                        "out_height %d\n", in_height, pos_y,
+                        out_height);
         }
 
-        if (!dss_feat_color_mode_supported(plane, oi->color_mode))
+        if (!dss_feat_color_mode_supported(plane, color_mode))
                 return -EINVAL;
 
-        r = dispc_ovl_calc_scaling(plane, channel, mgr_timings, in_width,
-                        in_height, out_width, out_height, oi->color_mode,
-                        &five_taps, &x_predecim, &y_predecim, oi->pos_x);
+        r = dispc_ovl_calc_scaling(plane, caps, mgr_timings, in_width,
+                        in_height, out_width, out_height, color_mode,
+                        &five_taps, &x_predecim, &y_predecim, pos_x,
+                        rotation_type, mem_to_mem);
         if (r)
                 return r;
 
         in_width = DIV_ROUND_UP(in_width, x_predecim);
         in_height = DIV_ROUND_UP(in_height, y_predecim);
 
-        if (oi->color_mode == OMAP_DSS_COLOR_YUV2 ||
-                        oi->color_mode == OMAP_DSS_COLOR_UYVY ||
-                        oi->color_mode == OMAP_DSS_COLOR_NV12)
+        if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY ||
+                        color_mode == OMAP_DSS_COLOR_NV12)
                 cconv = 1;
 
         if (ilace && !fieldmode) {
@@ -2235,70 +2438,144 @@ int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
         row_inc = 0;
         pix_inc = 0;
 
-        if (oi->rotation_type == OMAP_DSS_ROT_TILER)
-                calc_tiler_rotation_offset(oi->screen_width, in_width,
-                                oi->color_mode, fieldmode, field_offset,
+        if (rotation_type == OMAP_DSS_ROT_TILER)
+                calc_tiler_rotation_offset(screen_width, in_width,
+                                color_mode, fieldmode, field_offset,
                                 &offset0, &offset1, &row_inc, &pix_inc,
                                 x_predecim, y_predecim);
-        else if (oi->rotation_type == OMAP_DSS_ROT_DMA)
-                calc_dma_rotation_offset(oi->rotation, oi->mirror,
-                                oi->screen_width, in_width, frame_height,
-                                oi->color_mode, fieldmode, field_offset,
+        else if (rotation_type == OMAP_DSS_ROT_DMA)
+                calc_dma_rotation_offset(rotation, mirror,
+                                screen_width, in_width, frame_height,
+                                color_mode, fieldmode, field_offset,
                                 &offset0, &offset1, &row_inc, &pix_inc,
                                 x_predecim, y_predecim);
         else
-                calc_vrfb_rotation_offset(oi->rotation, oi->mirror,
-                                oi->screen_width, in_width, frame_height,
-                                oi->color_mode, fieldmode, field_offset,
+                calc_vrfb_rotation_offset(rotation, mirror,
+                                screen_width, in_width, frame_height,
+                                color_mode, fieldmode, field_offset,
                                 &offset0, &offset1, &row_inc, &pix_inc,
                                 x_predecim, y_predecim);
 
         DSSDBG("offset0 %u, offset1 %u, row_inc %d, pix_inc %d\n",
                         offset0, offset1, row_inc, pix_inc);
 
-        dispc_ovl_set_color_mode(plane, oi->color_mode);
+        dispc_ovl_set_color_mode(plane, color_mode);
 
-        dispc_ovl_configure_burst_type(plane, oi->rotation_type);
+        dispc_ovl_configure_burst_type(plane, rotation_type);
 
-        dispc_ovl_set_ba0(plane, oi->paddr + offset0);
-        dispc_ovl_set_ba1(plane, oi->paddr + offset1);
+        dispc_ovl_set_ba0(plane, paddr + offset0);
+        dispc_ovl_set_ba1(plane, paddr + offset1);
 
-        if (OMAP_DSS_COLOR_NV12 == oi->color_mode) {
-                dispc_ovl_set_ba0_uv(plane, oi->p_uv_addr + offset0);
-                dispc_ovl_set_ba1_uv(plane, oi->p_uv_addr + offset1);
+        if (OMAP_DSS_COLOR_NV12 == color_mode) {
+                dispc_ovl_set_ba0_uv(plane, p_uv_addr + offset0);
+                dispc_ovl_set_ba1_uv(plane, p_uv_addr + offset1);
         }
 
-
         dispc_ovl_set_row_inc(plane, row_inc);
         dispc_ovl_set_pix_inc(plane, pix_inc);
 
-        DSSDBG("%d,%d %dx%d -> %dx%d\n", oi->pos_x, oi->pos_y, in_width,
+        DSSDBG("%d,%d %dx%d -> %dx%d\n", pos_x, pos_y, in_width,
                         in_height, out_width, out_height);
 
-        dispc_ovl_set_pos(plane, oi->pos_x, oi->pos_y);
+        dispc_ovl_set_pos(plane, caps, pos_x, pos_y);
 
-        dispc_ovl_set_pic_size(plane, in_width, in_height);
+        dispc_ovl_set_input_size(plane, in_width, in_height);
 
-        if (ovl->caps & OMAP_DSS_OVL_CAP_SCALE) {
+        if (caps & OMAP_DSS_OVL_CAP_SCALE) {
                 dispc_ovl_set_scaling(plane, in_width, in_height, out_width,
                                    out_height, ilace, five_taps, fieldmode,
-                                   oi->color_mode, oi->rotation);
-                dispc_ovl_set_vid_size(plane, out_width, out_height);
+                                   color_mode, rotation);
+                dispc_ovl_set_output_size(plane, out_width, out_height);
                 dispc_ovl_set_vid_color_conv(plane, cconv);
         }
 
-        dispc_ovl_set_rotation_attrs(plane, oi->rotation, oi->mirror,
-                        oi->color_mode);
+        dispc_ovl_set_rotation_attrs(plane, rotation, mirror, color_mode);
 
-        dispc_ovl_set_zorder(plane, oi->zorder);
-        dispc_ovl_set_pre_mult_alpha(plane, oi->pre_mult_alpha);
-        dispc_ovl_setup_global_alpha(plane, oi->global_alpha);
+        dispc_ovl_set_zorder(plane, caps, zorder);
+        dispc_ovl_set_pre_mult_alpha(plane, caps, pre_mult_alpha);
+        dispc_ovl_setup_global_alpha(plane, caps, global_alpha);
 
-        dispc_ovl_enable_replication(plane, replication);
+        dispc_ovl_enable_replication(plane, caps, replication);
 
         return 0;
 }
 
+int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
+                bool replication, const struct omap_video_timings *mgr_timings,
+                bool mem_to_mem)
+{
+        int r;
+        struct omap_overlay *ovl = omap_dss_get_overlay(plane);
+        enum omap_channel channel;
+
+        channel = dispc_ovl_get_channel_out(plane);
+
+        DSSDBG("dispc_ovl_setup %d, pa %x, pa_uv %x, sw %d, %d,%d, %dx%d -> "
+                "%dx%d, cmode %x, rot %d, mir %d, chan %d repl %d\n",
+                plane, oi->paddr, oi->p_uv_addr, oi->screen_width, oi->pos_x,
+                oi->pos_y, oi->width, oi->height, oi->out_width, oi->out_height,
+                oi->color_mode, oi->rotation, oi->mirror, channel, replication);
+
+        r = dispc_ovl_setup_common(plane, ovl->caps, oi->paddr, oi->p_uv_addr,
+                oi->screen_width, oi->pos_x, oi->pos_y, oi->width, oi->height,
+                oi->out_width, oi->out_height, oi->color_mode, oi->rotation,
+                oi->mirror, oi->zorder, oi->pre_mult_alpha, oi->global_alpha,
+                oi->rotation_type, replication, mgr_timings, mem_to_mem);
+
+        return r;
+}
+
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+                bool mem_to_mem, const struct omap_video_timings *mgr_timings)
+{
+        int r;
+        u32 l;
+        enum omap_plane plane = OMAP_DSS_WB;
+        const int pos_x = 0, pos_y = 0;
+        const u8 zorder = 0, global_alpha = 0;
+        const bool replication = false;
+        bool truncation;
+        int in_width = mgr_timings->x_res;
+        int in_height = mgr_timings->y_res;
+        enum omap_overlay_caps caps =
+                OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA;
+
+        DSSDBG("dispc_wb_setup, pa %x, pa_uv %x, %d,%d -> %dx%d, cmode %x, "
+                "rot %d, mir %d\n", wi->paddr, wi->p_uv_addr, in_width,
+                in_height, wi->width, wi->height, wi->color_mode, wi->rotation,
+                wi->mirror);
+
+        r = dispc_ovl_setup_common(plane, caps, wi->paddr, wi->p_uv_addr,
+                wi->buf_width, pos_x, pos_y, in_width, in_height, wi->width,
+                wi->height, wi->color_mode, wi->rotation, wi->mirror, zorder,
+                wi->pre_mult_alpha, global_alpha, wi->rotation_type,
+                replication, mgr_timings, mem_to_mem);
+
+        switch (wi->color_mode) {
+        case OMAP_DSS_COLOR_RGB16:
+        case OMAP_DSS_COLOR_RGB24P:
+        case OMAP_DSS_COLOR_ARGB16:
+        case OMAP_DSS_COLOR_RGBA16:
+        case OMAP_DSS_COLOR_RGB12U:
+        case OMAP_DSS_COLOR_ARGB16_1555:
+        case OMAP_DSS_COLOR_XRGB16_1555:
+        case OMAP_DSS_COLOR_RGBX16:
+                truncation = true;
+                break;
+        default:
+                truncation = false;
+                break;
+        }
+
+        /* setup extra DISPC_WB_ATTRIBUTES */
+        l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+        l = FLD_MOD(l, truncation, 10, 10);     /* TRUNCATIONENABLE */
+        l = FLD_MOD(l, mem_to_mem, 19, 19);     /* WRITEBACKMODE */
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
+
+        return r;
+}
+
 int dispc_ovl_enable(enum omap_plane plane, bool enable)
 {
         DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
@@ -2451,6 +2728,47 @@ void dispc_mgr_enable(enum omap_channel channel, bool enable)
                 BUG();
 }
 
+void dispc_wb_enable(bool enable)
+{
+        enum omap_plane plane = OMAP_DSS_WB;
+        struct completion frame_done_completion;
+        bool is_on;
+        int r;
+        u32 irq;
+
+        is_on = REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0);
+        irq = DISPC_IRQ_FRAMEDONEWB;
+
+        if (!enable && is_on) {
+                init_completion(&frame_done_completion);
+
+                r = omap_dispc_register_isr(dispc_disable_isr,
+                                &frame_done_completion, irq);
+                if (r)
+                        DSSERR("failed to register FRAMEDONEWB isr\n");
+        }
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 0, 0);
+
+        if (!enable && is_on) {
+                if (!wait_for_completion_timeout(&frame_done_completion,
+                                        msecs_to_jiffies(100)))
+                        DSSERR("timeout waiting for FRAMEDONEWB\n");
+
+                r = omap_dispc_unregister_isr(dispc_disable_isr,
+                                &frame_done_completion, irq);
+                if (r)
+                        DSSERR("failed to unregister FRAMEDONEWB isr\n");
+        }
+}
+
+bool dispc_wb_is_enabled(void)
+{
+        enum omap_plane plane = OMAP_DSS_WB;
+
+        return REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0);
+}
+
 void dispc_lcd_enable_signal_polarity(bool act_high)
 {
         if (!dss_has_feature(FEAT_LCDENABLEPOL))
@@ -2605,24 +2923,13 @@ static bool _dispc_mgr_size_ok(u16 width, u16 height)
 static bool _dispc_lcd_timings_ok(int hsw, int hfp, int hbp,
                 int vsw, int vfp, int vbp)
 {
-        if (cpu_is_omap24xx() || omap_rev() < OMAP3430_REV_ES3_0) {
-                if (hsw < 1 || hsw > 64 ||
-                                hfp < 1 || hfp > 256 ||
-                                hbp < 1 || hbp > 256 ||
-                                vsw < 1 || vsw > 64 ||
-                                vfp < 0 || vfp > 255 ||
-                                vbp < 0 || vbp > 255)
-                        return false;
-        } else {
-                if (hsw < 1 || hsw > 256 ||
-                                hfp < 1 || hfp > 4096 ||
-                                hbp < 1 || hbp > 4096 ||
-                                vsw < 1 || vsw > 256 ||
-                                vfp < 0 || vfp > 4095 ||
-                                vbp < 0 || vbp > 4095)
-                        return false;
-        }
-
+        if (hsw < 1 || hsw > dispc.feat->sw_max ||
+                        hfp < 1 || hfp > dispc.feat->hp_max ||
+                        hbp < 1 || hbp > dispc.feat->hp_max ||
+                        vsw < 1 || vsw > dispc.feat->sw_max ||
+                        vfp < 0 || vfp > dispc.feat->vp_max ||
+                        vbp < 0 || vbp > dispc.feat->vp_max)
+                return false;
         return true;
 }
 
@@ -2654,19 +2961,12 @@ static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
         u32 timing_h, timing_v, l;
         bool onoff, rf, ipc;
 
-        if (cpu_is_omap24xx() || omap_rev() < OMAP3430_REV_ES3_0) {
-                timing_h = FLD_VAL(hsw-1, 5, 0) | FLD_VAL(hfp-1, 15, 8) |
-                        FLD_VAL(hbp-1, 27, 20);
-
-                timing_v = FLD_VAL(vsw-1, 5, 0) | FLD_VAL(vfp, 15, 8) |
-                        FLD_VAL(vbp, 27, 20);
-        } else {
-                timing_h = FLD_VAL(hsw-1, 7, 0) | FLD_VAL(hfp-1, 19, 8) |
-                        FLD_VAL(hbp-1, 31, 20);
-
-                timing_v = FLD_VAL(vsw-1, 7, 0) | FLD_VAL(vfp, 19, 8) |
-                        FLD_VAL(vbp, 31, 20);
-        }
+        timing_h = FLD_VAL(hsw-1, dispc.feat->sw_start, 0) |
+                        FLD_VAL(hfp-1, dispc.feat->fp_start, 8) |
+                        FLD_VAL(hbp-1, dispc.feat->bp_start, 20);
+        timing_v = FLD_VAL(vsw-1, dispc.feat->sw_start, 0) |
+                        FLD_VAL(vfp, dispc.feat->fp_start, 8) |
+                        FLD_VAL(vbp, dispc.feat->bp_start, 20);
 
         dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
         dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
@@ -2872,6 +3172,23 @@ unsigned long dispc_core_clk_rate(void)
         return fclk / lcd;
 }
 
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane)
+{
+        enum omap_channel channel = dispc_ovl_get_channel_out(plane);
+
+        return dispc_mgr_pclk_rate(channel);
+}
+
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane)
+{
+        enum omap_channel channel = dispc_ovl_get_channel_out(plane);
+
+        if (dss_mgr_is_lcd(channel))
+                return dispc_mgr_lclk_rate(channel);
+        else
+                return dispc_fclk_rate();
+
+}
 static void dispc_dump_clocks_channel(struct seq_file *s, enum omap_channel channel)
 {
         int lcd, pcd;
@@ -3492,7 +3809,7 @@ static void dispc_error_worker(struct work_struct *work)
                                         ovl->name);
                         dispc_ovl_enable(ovl->id, false);
                         dispc_mgr_go(ovl->manager->id);
-                        mdelay(50);
+                        msleep(50);
                 }
         }
 
@@ -3504,7 +3821,7 @@ static void dispc_error_worker(struct work_struct *work)
                 bit = mgr_desc[i].sync_lost_irq;
 
                 if (bit & errors) {
-                        struct omap_dss_device *dssdev = mgr->device;
+                        struct omap_dss_device *dssdev = mgr->get_device(mgr);
                         bool enable;
 
                         DSSERR("SYNC_LOST on channel %s, restarting the output "
@@ -3524,7 +3841,7 @@ static void dispc_error_worker(struct work_struct *work)
                         }
 
                         dispc_mgr_go(mgr->id);
-                        mdelay(50);
+                        msleep(50);
 
                         if (enable)
                                 dssdev->driver->enable(dssdev);
@@ -3535,9 +3852,13 @@ static void dispc_error_worker(struct work_struct *work)
                 DSSERR("OCP_ERR\n");
                 for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
                         struct omap_overlay_manager *mgr;
+                        struct omap_dss_device *dssdev;
+
                         mgr = omap_dss_get_overlay_manager(i);
-                        if (mgr->device && mgr->device->driver)
-                                mgr->device->driver->disable(mgr->device);
+                        dssdev = mgr->get_device(mgr);
+
+                        if (dssdev && dssdev->driver)
+                                dssdev->driver->disable(dssdev);
                 }
         }
 
@@ -3661,17 +3982,98 @@ static void _omap_dispc_initial_config(void)
         if (dss_has_feature(FEAT_FUNCGATED))
                 REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
 
-        _dispc_setup_color_conv_coef();
+        dispc_setup_color_conv_coef();
 
         dispc_set_loadmode(OMAP_DSS_LOAD_FRAME_ONLY);
 
-        dispc_read_plane_fifo_sizes();
+        dispc_init_fifos();
 
         dispc_configure_burst_sizes();
 
         dispc_ovl_enable_zorder_planes();
 }
 
+static const struct dispc_features omap24xx_dispc_feats __initconst = {
+        .sw_start               =       5,
+        .fp_start               =       15,
+        .bp_start               =       27,
+        .sw_max                 =       64,
+        .vp_max                 =       255,
+        .hp_max                 =       256,
+        .calc_scaling           =       dispc_ovl_calc_scaling_24xx,
+        .calc_core_clk          =       calc_core_clk_24xx,
+        .num_fifos              =       3,
+};
+
+static const struct dispc_features omap34xx_rev1_0_dispc_feats __initconst = {
+        .sw_start               =       5,
+        .fp_start               =       15,
+        .bp_start               =       27,
+        .sw_max                 =       64,
+        .vp_max                 =       255,
+        .hp_max                 =       256,
+        .calc_scaling           =       dispc_ovl_calc_scaling_34xx,
+        .calc_core_clk          =       calc_core_clk_34xx,
+        .num_fifos              =       3,
+};
+
+static const struct dispc_features omap34xx_rev3_0_dispc_feats __initconst = {
+        .sw_start               =       7,
+        .fp_start               =       19,
+        .bp_start               =       31,
+        .sw_max                 =       256,
+        .vp_max                 =       4095,
+        .hp_max                 =       4096,
+        .calc_scaling           =       dispc_ovl_calc_scaling_34xx,
+        .calc_core_clk          =       calc_core_clk_34xx,
+        .num_fifos              =       3,
+};
+
+static const struct dispc_features omap44xx_dispc_feats __initconst = {
+        .sw_start               =       7,
+        .fp_start               =       19,
+        .bp_start               =       31,
+        .sw_max                 =       256,
+        .vp_max                 =       4095,
+        .hp_max                 =       4096,
+        .calc_scaling           =       dispc_ovl_calc_scaling_44xx,
+        .calc_core_clk          =       calc_core_clk_44xx,
+        .num_fifos              =       5,
+        .gfx_fifo_workaround    =       true,
+};
+
+static int __init dispc_init_features(struct device *dev)
+{
+        const struct dispc_features *src;
+        struct dispc_features *dst;
+
+        dst = devm_kzalloc(dev, sizeof(*dst), GFP_KERNEL);
+        if (!dst) {
+                dev_err(dev, "Failed to allocate DISPC Features\n");
+                return -ENOMEM;
+        }
+
+        if (cpu_is_omap24xx()) {
+                src = &omap24xx_dispc_feats;
+        } else if (cpu_is_omap34xx()) {
+                if (omap_rev() < OMAP3430_REV_ES3_0)
+                        src = &omap34xx_rev1_0_dispc_feats;
+                else
+                        src = &omap34xx_rev3_0_dispc_feats;
+        } else if (cpu_is_omap44xx()) {
+                src = &omap44xx_dispc_feats;
+        } else if (soc_is_omap54xx()) {
+                src = &omap44xx_dispc_feats;
+        } else {
+                return -ENODEV;
+        }
+
+        memcpy(dst, src, sizeof(*dst));
+        dispc.feat = dst;
+
+        return 0;
+}
+
 /* DISPC HW IP initialisation */
 static int __init omap_dispchw_probe(struct platform_device *pdev)
 {
@@ -3682,6 +4084,10 @@ static int __init omap_dispchw_probe(struct platform_device *pdev)
 
         dispc.pdev = pdev;
 
+        r = dispc_init_features(&dispc.pdev->dev);
+        if (r)
+                return r;
+
         spin_lock_init(&dispc.irq_lock);
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
diff --git a/drivers/video/omap2/dss/dispc.h b/drivers/video/omap2/dss/dispc.h
index 92d8a9be86fc..222363c6e623 100644
--- a/drivers/video/omap2/dss/dispc.h
+++ b/drivers/video/omap2/dss/dispc.h
@@ -36,6 +36,7 @@
 #define DISPC_CONTROL2                  0x0238
 #define DISPC_CONFIG2                   0x0620
 #define DISPC_DIVISOR                   0x0804
+#define DISPC_GLOBAL_BUFFER             0x0800
 #define DISPC_CONTROL3                  0x0848
 #define DISPC_CONFIG3                   0x084C
 
@@ -355,6 +356,8 @@ static inline u16 DISPC_OVL_BASE(enum omap_plane plane)
                 return 0x014C;
         case OMAP_DSS_VIDEO3:
                 return 0x0300;
+        case OMAP_DSS_WB:
+                return 0x0500;
         default:
                 BUG();
                 return 0;
@@ -370,6 +373,7 @@ static inline u16 DISPC_BA0_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0000;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0008;
         default:
                 BUG();
@@ -385,6 +389,7 @@ static inline u16 DISPC_BA1_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0004;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x000C;
         default:
                 BUG();
@@ -404,6 +409,8 @@ static inline u16 DISPC_BA0_UV_OFFSET(enum omap_plane plane)
                 return 0x04BC;
         case OMAP_DSS_VIDEO3:
                 return 0x0310;
+        case OMAP_DSS_WB:
+                return 0x0118;
         default:
                 BUG();
                 return 0;
@@ -422,6 +429,8 @@ static inline u16 DISPC_BA1_UV_OFFSET(enum omap_plane plane)
                 return 0x04C0;
         case OMAP_DSS_VIDEO3:
                 return 0x0314;
+        case OMAP_DSS_WB:
+                return 0x011C;
         default:
                 BUG();
                 return 0;
@@ -451,6 +460,7 @@ static inline u16 DISPC_SIZE_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x000C;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x00A8;
         default:
                 BUG();
@@ -467,6 +477,7 @@ static inline u16 DISPC_ATTR_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0010;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0070;
         default:
                 BUG();
@@ -486,6 +497,8 @@ static inline u16 DISPC_ATTR2_OFFSET(enum omap_plane plane)
                 return 0x04DC;
         case OMAP_DSS_VIDEO3:
                 return 0x032C;
+        case OMAP_DSS_WB:
+                return 0x0310;
         default:
                 BUG();
                 return 0;
@@ -501,6 +514,7 @@ static inline u16 DISPC_FIFO_THRESH_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0014;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x008C;
         default:
                 BUG();
@@ -517,6 +531,7 @@ static inline u16 DISPC_FIFO_SIZE_STATUS_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0018;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0088;
         default:
                 BUG();
@@ -533,6 +548,7 @@ static inline u16 DISPC_ROW_INC_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x001C;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x00A4;
         default:
                 BUG();
@@ -549,6 +565,7 @@ static inline u16 DISPC_PIX_INC_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0020;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0098;
         default:
                 BUG();
@@ -598,6 +615,7 @@ static inline u16 DISPC_FIR_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0024;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0090;
         default:
                 BUG();
@@ -617,6 +635,8 @@ static inline u16 DISPC_FIR2_OFFSET(enum omap_plane plane)
                 return 0x055C;
         case OMAP_DSS_VIDEO3:
                 return 0x0424;
+        case OMAP_DSS_WB:
+                return 0x290;
         default:
                 BUG();
                 return 0;
@@ -633,6 +653,7 @@ static inline u16 DISPC_PIC_SIZE_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0028;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0094;
         default:
                 BUG();
@@ -651,6 +672,7 @@ static inline u16 DISPC_ACCU0_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x002C;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0000;
         default:
                 BUG();
@@ -670,6 +692,8 @@ static inline u16 DISPC_ACCU2_0_OFFSET(enum omap_plane plane)
                 return 0x0560;
         case OMAP_DSS_VIDEO3:
                 return 0x0428;
+        case OMAP_DSS_WB:
+                return 0x0294;
         default:
                 BUG();
                 return 0;
@@ -686,6 +710,7 @@ static inline u16 DISPC_ACCU1_OFFSET(enum omap_plane plane)
         case OMAP_DSS_VIDEO2:
                 return 0x0030;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0004;
         default:
                 BUG();
@@ -705,6 +730,8 @@ static inline u16 DISPC_ACCU2_1_OFFSET(enum omap_plane plane)
                 return 0x0564;
         case OMAP_DSS_VIDEO3:
                 return 0x042C;
+        case OMAP_DSS_WB:
+                return 0x0298;
         default:
                 BUG();
                 return 0;
@@ -722,6 +749,7 @@ static inline u16 DISPC_FIR_COEF_H_OFFSET(enum omap_plane plane, u16 i)
         case OMAP_DSS_VIDEO2:
                 return 0x0034 + i * 0x8;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0010 + i * 0x8;
         default:
                 BUG();
@@ -742,6 +770,8 @@ static inline u16 DISPC_FIR_COEF_H2_OFFSET(enum omap_plane plane, u16 i)
                 return 0x0568 + i * 0x8;
         case OMAP_DSS_VIDEO3:
                 return 0x0430 + i * 0x8;
+        case OMAP_DSS_WB:
+                return 0x02A0 + i * 0x8;
         default:
                 BUG();
                 return 0;
@@ -759,6 +789,7 @@ static inline u16 DISPC_FIR_COEF_HV_OFFSET(enum omap_plane plane, u16 i)
         case OMAP_DSS_VIDEO2:
                 return 0x0038 + i * 0x8;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0014 + i * 0x8;
         default:
                 BUG();
@@ -779,6 +810,8 @@ static inline u16 DISPC_FIR_COEF_HV2_OFFSET(enum omap_plane plane, u16 i)
                 return 0x056C + i * 0x8;
         case OMAP_DSS_VIDEO3:
                 return 0x0434 + i * 0x8;
+        case OMAP_DSS_WB:
+                return 0x02A4 + i * 0x8;
         default:
                 BUG();
                 return 0;
@@ -795,6 +828,7 @@ static inline u16 DISPC_CONV_COEF_OFFSET(enum omap_plane plane, u16 i)
         case OMAP_DSS_VIDEO1:
         case OMAP_DSS_VIDEO2:
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0074 + i * 0x4;
         default:
                 BUG();
@@ -814,6 +848,7 @@ static inline u16 DISPC_FIR_COEF_V_OFFSET(enum omap_plane plane, u16 i)
         case OMAP_DSS_VIDEO2:
                 return 0x00B4 + i * 0x4;
         case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
                 return 0x0050 + i * 0x4;
         default:
                 BUG();
@@ -834,6 +869,8 @@ static inline u16 DISPC_FIR_COEF_V2_OFFSET(enum omap_plane plane, u16 i)
                 return 0x05A8 + i * 0x4;
         case OMAP_DSS_VIDEO3:
                 return 0x0470 + i * 0x4;
+        case OMAP_DSS_WB:
+                return 0x02E0 + i * 0x4;
         default:
                 BUG();
                 return 0;
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 5bd957e85505..ccf8550fafde 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -142,7 +142,11 @@ static ssize_t display_timings_store(struct device *dev,
         if (r)
                 return r;
 
+        dssdev->driver->disable(dssdev);
         dssdev->driver->set_timings(dssdev, &t);
+        r = dssdev->driver->enable(dssdev);
+        if (r)
+                return r;
 
         return size;
 }
@@ -316,26 +320,117 @@ void omapdss_default_get_timings(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(omapdss_default_get_timings);
 
-void dss_init_device(struct platform_device *pdev,
+/*
+ * Connect dssdev to a manager if the manager is free or if force is specified.
+ * Connect all overlays to that manager if they are free or if force is
+ * specified.
+ */
+static int dss_init_connections(struct omap_dss_device *dssdev, bool force)
+{
+        struct omap_dss_output *out;
+        struct omap_overlay_manager *mgr;
+        int i, r;
+
+        out = omapdss_get_output_from_dssdev(dssdev);
+
+        WARN_ON(dssdev->output);
+        WARN_ON(out->device);
+
+        r = omapdss_output_set_device(out, dssdev);
+        if (r) {
+                DSSERR("failed to connect output to new device\n");
+                return r;
+        }
+
+        mgr = omap_dss_get_overlay_manager(dssdev->channel);
+
+        if (mgr->output && !force)
+                return 0;
+
+        if (mgr->output)
+                mgr->unset_output(mgr);
+
+        r = mgr->set_output(mgr, out);
+        if (r) {
+                DSSERR("failed to connect manager to output of new device\n");
+
+                /* remove the output-device connection we just made */
+                omapdss_output_unset_device(out);
+                return r;
+        }
+
+        for (i = 0; i < omap_dss_get_num_overlays(); ++i) {
+                struct omap_overlay *ovl = omap_dss_get_overlay(i);
+
+                if (!ovl->manager || force) {
+                        if (ovl->manager)
+                                ovl->unset_manager(ovl);
+
+                        r = ovl->set_manager(ovl, mgr);
+                        if (r) {
+                                DSSERR("failed to set initial overlay\n");
+                                return r;
+                        }
+                }
+        }
+
+        return 0;
+}
+
+static void dss_uninit_connections(struct omap_dss_device *dssdev)
+{
+        if (dssdev->output) {
+                struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+                if (mgr)
+                        mgr->unset_output(mgr);
+
+                omapdss_output_unset_device(dssdev->output);
+        }
+}
+
+int dss_init_device(struct platform_device *pdev,
                 struct omap_dss_device *dssdev)
 {
         struct device_attribute *attr;
-        int i;
-        int r;
+        int i, r;
+        const char *def_disp_name = dss_get_default_display_name();
+        bool force;
+
+        force = def_disp_name && strcmp(def_disp_name, dssdev->name) == 0;
+        dss_init_connections(dssdev, force);
 
         /* create device sysfs files */
         i = 0;
         while ((attr = display_sysfs_attrs[i++]) != NULL) {
                 r = device_create_file(&dssdev->dev, attr);
-                if (r)
+                if (r) {
+                        for (i = i - 2; i >= 0; i--) {
+                                attr = display_sysfs_attrs[i];
+                                device_remove_file(&dssdev->dev, attr);
+                        }
+
+                        dss_uninit_connections(dssdev);
+
                         DSSERR("failed to create sysfs file\n");
+                        return r;
+                }
         }
 
         /* create display? sysfs links */
         r = sysfs_create_link(&pdev->dev.kobj, &dssdev->dev.kobj,
                         dev_name(&dssdev->dev));
-        if (r)
+        if (r) {
+                while ((attr = display_sysfs_attrs[i++]) != NULL)
+                        device_remove_file(&dssdev->dev, attr);
+
+                dss_uninit_connections(dssdev);
+
                 DSSERR("failed to create sysfs display link\n");
+                return r;
+        }
+
+        return 0;
 }
 
 void dss_uninit_device(struct platform_device *pdev,
@@ -349,8 +444,7 @@ void dss_uninit_device(struct platform_device *pdev,
         while ((attr = display_sysfs_attrs[i++]) != NULL)
                 device_remove_file(&dssdev->dev, attr);
 
-        if (dssdev->manager)
-                dssdev->manager->unset_device(dssdev->manager);
+        dss_uninit_connections(dssdev);
 }
 
 static int dss_suspend_device(struct device *dev, void *data)
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 3266be23fc0d..56748cf8760e 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -29,17 +29,24 @@
 #include <linux/errno.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
+#include <linux/string.h>
 
 #include <video/omapdss.h>
-#include <plat/cpu.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static struct {
         struct regulator *vdds_dsi_reg;
         struct platform_device *dsidev;
 
+        struct mutex lock;
+
+        struct omap_video_timings timings;
         struct dss_lcd_mgr_config mgr_config;
+        int data_lines;
+
+        struct omap_dss_output output;
 } dpi;
 
 static struct platform_device *dpi_get_dsidev(enum omap_dss_clk_source clk)
@@ -121,7 +128,8 @@ static int dpi_set_dispc_clk(struct omap_dss_device *dssdev,
 
 static int dpi_set_mode(struct omap_dss_device *dssdev)
 {
-        struct omap_video_timings *t = &dssdev->panel.timings;
+        struct omap_video_timings *t = &dpi.timings;
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         int lck_div = 0, pck_div = 0;
         unsigned long fck = 0;
         unsigned long pck;
@@ -146,37 +154,44 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
                 t->pixel_clock = pck;
         }
 
-        dss_mgr_set_timings(dssdev->manager, t);
+        dss_mgr_set_timings(mgr, t);
 
         return 0;
 }
 
 static void dpi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
         dpi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
 
         dpi.mgr_config.stallmode = false;
         dpi.mgr_config.fifohandcheck = false;
 
-        dpi.mgr_config.video_port_width = dssdev->phy.dpi.data_lines;
+        dpi.mgr_config.video_port_width = dpi.data_lines;
 
         dpi.mgr_config.lcden_sig_polarity = 0;
 
-        dss_mgr_set_lcd_config(dssdev->manager, &dpi.mgr_config);
+        dss_mgr_set_lcd_config(mgr, &dpi.mgr_config);
 }
 
 int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 {
+        struct omap_dss_output *out = dssdev->output;
         int r;
 
-        if (cpu_is_omap34xx() && !dpi.vdds_dsi_reg) {
+        mutex_lock(&dpi.lock);
+
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI) && !dpi.vdds_dsi_reg) {
                 DSSERR("no VDSS_DSI regulator\n");
-                return -ENODEV;
+                r = -ENODEV;
+                goto err_no_reg;
         }
 
-        if (dssdev->manager == NULL) {
-                DSSERR("failed to enable display: no manager\n");
-                return -ENODEV;
+        if (out == NULL || out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                r = -ENODEV;
+                goto err_no_out_mgr;
         }
 
         r = omap_dss_start_device(dssdev);
@@ -185,7 +200,7 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
                 goto err_start_dev;
         }
 
-        if (cpu_is_omap34xx()) {
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI)) {
                 r = regulator_enable(dpi.vdds_dsi_reg);
                 if (r)
                         goto err_reg_enable;
@@ -195,6 +210,10 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
         if (r)
                 goto err_get_dispc;
 
+        r = dss_dpi_select_source(dssdev->channel);
+        if (r)
+                goto err_src_sel;
+
         if (dpi_use_dsi_pll(dssdev)) {
                 r = dsi_runtime_get(dpi.dsidev);
                 if (r)
@@ -213,10 +232,12 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 
         mdelay(2);
 
-        r = dss_mgr_enable(dssdev->manager);
+        r = dss_mgr_enable(out->manager);
         if (r)
                 goto err_mgr_enable;
 
+        mutex_unlock(&dpi.lock);
+
         return 0;
 
 err_mgr_enable:
@@ -227,20 +248,28 @@ err_dsi_pll_init:
         if (dpi_use_dsi_pll(dssdev))
                 dsi_runtime_put(dpi.dsidev);
 err_get_dsi:
+err_src_sel:
         dispc_runtime_put();
 err_get_dispc:
-        if (cpu_is_omap34xx())
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
                 regulator_disable(dpi.vdds_dsi_reg);
 err_reg_enable:
         omap_dss_stop_device(dssdev);
 err_start_dev:
+err_no_out_mgr:
+err_no_reg:
+        mutex_unlock(&dpi.lock);
         return r;
 }
 EXPORT_SYMBOL(omapdss_dpi_display_enable);
 
 void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 {
-        dss_mgr_disable(dssdev->manager);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+        mutex_lock(&dpi.lock);
+
+        dss_mgr_disable(mgr);
 
         if (dpi_use_dsi_pll(dssdev)) {
                 dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
@@ -250,44 +279,39 @@ void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 
         dispc_runtime_put();
 
-        if (cpu_is_omap34xx())
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
                 regulator_disable(dpi.vdds_dsi_reg);
 
         omap_dss_stop_device(dssdev);
+
+        mutex_unlock(&dpi.lock);
 }
 EXPORT_SYMBOL(omapdss_dpi_display_disable);
 
-void dpi_set_timings(struct omap_dss_device *dssdev,
-                        struct omap_video_timings *timings)
+void omapdss_dpi_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
 {
-        int r;
-
         DSSDBG("dpi_set_timings\n");
-        dssdev->panel.timings = *timings;
-        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-                r = dispc_runtime_get();
-                if (r)
-                        return;
 
-                dpi_set_mode(dssdev);
+        mutex_lock(&dpi.lock);
 
-                dispc_runtime_put();
-        } else {
-                dss_mgr_set_timings(dssdev->manager, timings);
-        }
+        dpi.timings = *timings;
+
+        mutex_unlock(&dpi.lock);
 }
-EXPORT_SYMBOL(dpi_set_timings);
+EXPORT_SYMBOL(omapdss_dpi_set_timings);
 
 int dpi_check_timings(struct omap_dss_device *dssdev,
                         struct omap_video_timings *timings)
 {
         int r;
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         int lck_div, pck_div;
         unsigned long fck;
         unsigned long pck;
         struct dispc_clock_info dispc_cinfo;
 
-        if (dss_mgr_check_timings(dssdev->manager, timings))
+        if (dss_mgr_check_timings(mgr, timings))
                 return -EINVAL;
 
         if (timings->pixel_clock == 0)
@@ -325,11 +349,22 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(dpi_check_timings);
 
+void omapdss_dpi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+        mutex_lock(&dpi.lock);
+
+        dpi.data_lines = data_lines;
+
+        mutex_unlock(&dpi.lock);
+}
+EXPORT_SYMBOL(omapdss_dpi_set_data_lines);
+
 static int __init dpi_init_display(struct omap_dss_device *dssdev)
 {
         DSSDBG("init_display\n");
 
-        if (cpu_is_omap34xx() && dpi.vdds_dsi_reg == NULL) {
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI) &&
+                                        dpi.vdds_dsi_reg == NULL) {
                 struct regulator *vdds_dsi;
 
                 vdds_dsi = dss_get_vdds_dsi();
@@ -351,10 +386,14 @@ static int __init dpi_init_display(struct omap_dss_device *dssdev)
         return 0;
 }
 
-static void __init dpi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init dpi_find_dssdev(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int i, r;
+        const char *def_disp_name = dss_get_default_display_name();
+        struct omap_dss_device *def_dssdev;
+        int i;
+
+        def_dssdev = NULL;
 
         for (i = 0; i < pdata->num_devices; ++i) {
                 struct omap_dss_device *dssdev = pdata->devices[i];
@@ -362,21 +401,75 @@ static void __init dpi_probe_pdata(struct platform_device *pdev)
                 if (dssdev->type != OMAP_DISPLAY_TYPE_DPI)
                         continue;
 
-                r = dpi_init_display(dssdev);
-                if (r) {
-                        DSSERR("device %s init failed: %d\n", dssdev->name, r);
-                        continue;
+                if (def_dssdev == NULL)
+                        def_dssdev = dssdev;
+
+                if (def_disp_name != NULL &&
+                                strcmp(dssdev->name, def_disp_name) == 0) {
+                        def_dssdev = dssdev;
+                        break;
                 }
+        }
 
-                r = omap_dss_register_device(dssdev, &pdev->dev, i);
-                if (r)
-                        DSSERR("device %s register failed: %d\n",
-                                        dssdev->name, r);
+        return def_dssdev;
+}
+
+static void __init dpi_probe_pdata(struct platform_device *dpidev)
+{
+        struct omap_dss_device *plat_dssdev;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        plat_dssdev = dpi_find_dssdev(dpidev);
+
+        if (!plat_dssdev)
+                return;
+
+        dssdev = dss_alloc_and_init_device(&dpidev->dev);
+        if (!dssdev)
+                return;
+
+        dss_copy_device_pdata(dssdev, plat_dssdev);
+
+        r = dpi_init_display(dssdev);
+        if (r) {
+                DSSERR("device %s init failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
+        }
+
+        r = dss_add_device(dssdev);
+        if (r) {
+                DSSERR("device %s register failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
         }
 }
 
+static void __init dpi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &dpi.output;
+
+        out->pdev = pdev;
+        out->id = OMAP_DSS_OUTPUT_DPI;
+        out->type = OMAP_DISPLAY_TYPE_DPI;
+
+        dss_register_output(out);
+}
+
+static void __exit dpi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &dpi.output;
+
+        dss_unregister_output(out);
+}
+
 static int __init omap_dpi_probe(struct platform_device *pdev)
 {
+        mutex_init(&dpi.lock);
+
+        dpi_init_output(pdev);
+
         dpi_probe_pdata(pdev);
 
         return 0;
@@ -384,7 +477,9 @@ static int __init omap_dpi_probe(struct platform_device *pdev)
 
 static int __exit omap_dpi_remove(struct platform_device *pdev)
 {
-        omap_dss_unregister_child_devices(&pdev->dev);
+        dss_unregister_child_devices(&pdev->dev);
+
+        dpi_uninit_output(pdev);
 
         return 0;
 }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 05ee04667af1..d64ac3842884 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -41,7 +41,6 @@
 
 #include <video/omapdss.h>
 #include <video/mipi_display.h>
-#include <plat/clock.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -333,6 +332,12 @@ struct dsi_data {
         unsigned scp_clk_refcount;
 
         struct dss_lcd_mgr_config mgr_config;
+        struct omap_video_timings timings;
+        enum omap_dss_dsi_pixel_format pix_fmt;
+        enum omap_dss_dsi_mode mode;
+        struct omap_dss_dsi_videomode_timings vm_timings;
+
+        struct omap_dss_output output;
 };
 
 struct dsi_packet_sent_handler_data {
@@ -340,8 +345,6 @@ struct dsi_packet_sent_handler_data {
         struct completion *completion;
 };
 
-static struct platform_device *dsi_pdev_map[MAX_NUM_DSI];
-
 #ifdef DEBUG
 static bool dsi_perf;
 module_param(dsi_perf, bool, 0644);
@@ -354,12 +357,19 @@ static inline struct dsi_data *dsi_get_dsidrv_data(struct platform_device *dside
 
 static inline struct platform_device *dsi_get_dsidev_from_dssdev(struct omap_dss_device *dssdev)
 {
-        return dsi_pdev_map[dssdev->phy.dsi.module];
+        return dssdev->output->pdev;
 }
 
 struct platform_device *dsi_get_dsidev_from_id(int module)
 {
-        return dsi_pdev_map[module];
+        struct omap_dss_output *out;
+        enum omap_dss_output_id id;
+
+        id = module == 0 ? OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
+
+        out = omap_dss_get_output(id);
+
+        return out->pdev;
 }
 
 static inline void dsi_write_reg(struct platform_device *dsidev,
@@ -1450,6 +1460,148 @@ found:
         return 0;
 }
 
+static int dsi_pll_calc_ddrfreq(struct platform_device *dsidev,
+                unsigned long req_clkin4ddr, struct dsi_clock_info *cinfo)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dsi_clock_info cur, best;
+
+        DSSDBG("dsi_pll_calc_ddrfreq\n");
+
+        memset(&best, 0, sizeof(best));
+        memset(&cur, 0, sizeof(cur));
+
+        cur.clkin = clk_get_rate(dsi->sys_clk);
+
+        for (cur.regn = 1; cur.regn < dsi->regn_max; ++cur.regn) {
+                cur.fint = cur.clkin / cur.regn;
+
+                if (cur.fint > dsi->fint_max || cur.fint < dsi->fint_min)
+                        continue;
+
+                /* DSIPHY(MHz) = (2 * regm / regn) * clkin */
+                for (cur.regm = 1; cur.regm < dsi->regm_max; ++cur.regm) {
+                        unsigned long a, b;
+
+                        a = 2 * cur.regm * (cur.clkin/1000);
+                        b = cur.regn;
+                        cur.clkin4ddr = a / b * 1000;
+
+                        if (cur.clkin4ddr > 1800 * 1000 * 1000)
+                                break;
+
+                        if (abs(cur.clkin4ddr - req_clkin4ddr) <
+                                        abs(best.clkin4ddr - req_clkin4ddr)) {
+                                best = cur;
+                                DSSDBG("best %ld\n", best.clkin4ddr);
+                        }
+
+                        if (cur.clkin4ddr == req_clkin4ddr)
+                                goto found;
+                }
+        }
+found:
+        if (cinfo)
+                *cinfo = best;
+
+        return 0;
+}
+
+static void dsi_pll_calc_dsi_fck(struct platform_device *dsidev,
+                struct dsi_clock_info *cinfo)
+{
+        unsigned long max_dsi_fck;
+
+        max_dsi_fck = dss_feat_get_param_max(FEAT_PARAM_DSI_FCK);
+
+        cinfo->regm_dsi = DIV_ROUND_UP(cinfo->clkin4ddr, max_dsi_fck);
+        cinfo->dsi_pll_hsdiv_dsi_clk = cinfo->clkin4ddr / cinfo->regm_dsi;
+}
+
+static int dsi_pll_calc_dispc_fck(struct platform_device *dsidev,
+                unsigned long req_pck, struct dsi_clock_info *cinfo,
+                struct dispc_clock_info *dispc_cinfo)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned regm_dispc, best_regm_dispc;
+        unsigned long dispc_clk, best_dispc_clk;
+        int min_fck_per_pck;
+        unsigned long max_dss_fck;
+        struct dispc_clock_info best_dispc;
+        bool match;
+
+        max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+        min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
+
+        if (min_fck_per_pck &&
+                        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");
+                min_fck_per_pck = 0;
+        }
+
+retry:
+        best_regm_dispc = 0;
+        best_dispc_clk = 0;
+        memset(&best_dispc, 0, sizeof(best_dispc));
+        match = false;
+
+        for (regm_dispc = 1; regm_dispc < dsi->regm_dispc_max; ++regm_dispc) {
+                struct dispc_clock_info cur_dispc;
+
+                dispc_clk = cinfo->clkin4ddr / regm_dispc;
+
+                /* this will narrow down the search a bit,
+                 * but still give pixclocks below what was
+                 * requested */
+                if (dispc_clk  < req_pck)
+                        break;
+
+                if (dispc_clk > max_dss_fck)
+                        continue;
+
+                if (min_fck_per_pck && dispc_clk < req_pck * min_fck_per_pck)
+                        continue;
+
+                match = true;
+
+                dispc_find_clk_divs(req_pck, dispc_clk, &cur_dispc);
+
+                if (abs(cur_dispc.pck - req_pck) <
+                                abs(best_dispc.pck - req_pck)) {
+                        best_regm_dispc = regm_dispc;
+                        best_dispc_clk = dispc_clk;
+                        best_dispc = cur_dispc;
+
+                        if (cur_dispc.pck == req_pck)
+                                goto found;
+                }
+        }
+
+        if (!match) {
+                if (min_fck_per_pck) {
+                        DSSERR("Could not find suitable clock settings.\n"
+                                        "Turning FCK/PCK constraint off and"
+                                        "trying again.\n");
+                        min_fck_per_pck = 0;
+                        goto retry;
+                }
+
+                DSSERR("Could not find suitable clock settings.\n");
+
+                return -EINVAL;
+        }
+found:
+        cinfo->regm_dispc = best_regm_dispc;
+        cinfo->dsi_pll_hsdiv_dispc_clk = best_dispc_clk;
+
+        *dispc_cinfo = best_dispc;
+
+        return 0;
+}
+
 int dsi_pll_set_clock_div(struct platform_device *dsidev,
                 struct dsi_clock_info *cinfo)
 {
@@ -1526,21 +1678,27 @@ int dsi_pll_set_clock_div(struct platform_device *dsidev,
 
         BUG_ON(cinfo->fint < dsi->fint_min || cinfo->fint > dsi->fint_max);
 
+        l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
+
         if (dss_has_feature(FEAT_DSI_PLL_FREQSEL)) {
                 f = cinfo->fint < 1000000 ? 0x3 :
                         cinfo->fint < 1250000 ? 0x4 :
                         cinfo->fint < 1500000 ? 0x5 :
                         cinfo->fint < 1750000 ? 0x6 :
                         0x7;
-        }
 
-        l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
-
-        if (dss_has_feature(FEAT_DSI_PLL_FREQSEL))
                 l = FLD_MOD(l, f, 4, 1);        /* DSI_PLL_FREQSEL */
+        } else if (dss_has_feature(FEAT_DSI_PLL_SELFREQDCO)) {
+                f = cinfo->clkin4ddr < 1000000000 ? 0x2 : 0x4;
+
+                l = FLD_MOD(l, f, 4, 1);        /* PLL_SELFREQDCO */
+        }
+
         l = FLD_MOD(l, 1, 13, 13);              /* DSI_PLL_REFEN */
         l = FLD_MOD(l, 0, 14, 14);              /* DSIPHY_CLKINEN */
         l = FLD_MOD(l, 1, 20, 20);              /* DSI_HSDIVBYPASS */
+        if (dss_has_feature(FEAT_DSI_PLL_REFSEL))
+                l = FLD_MOD(l, 3, 22, 21);      /* REF_SYSCLK = sysclk */
         dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION2, l);
 
         REG_FLD_MOD(dsidev, DSI_PLL_GO, 1, 0, 0);       /* DSI_PLL_GO */
@@ -2004,15 +2162,16 @@ static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
                 return 1194 * 3;        /* 1194x24 bits */
         case 6:
                 return 1365 * 3;        /* 1365x24 bits */
+        case 7:
+                return 1920 * 3;        /* 1920x24 bits */
         default:
                 BUG();
                 return 0;
         }
 }
 
-static int dsi_set_lane_config(struct omap_dss_device *dssdev)
+static int dsi_set_lane_config(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         static const u8 offsets[] = { 0, 4, 8, 12, 16 };
         static const enum dsi_lane_function functions[] = {
@@ -2136,9 +2295,16 @@ static void dsi_cio_timings(struct platform_device *dsidev)
         dsi_write_reg(dsidev, DSI_DSIPHY_CFG0, r);
 
         r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
-        r = FLD_MOD(r, tlpx_half, 22, 16);
+        r = FLD_MOD(r, tlpx_half, 20, 16);
         r = FLD_MOD(r, tclk_trail, 15, 8);
         r = FLD_MOD(r, tclk_zero, 7, 0);
+
+        if (dss_has_feature(FEAT_DSI_PHY_DCC)) {
+                r = FLD_MOD(r, 0, 21, 21);      /* DCCEN = disable */
+                r = FLD_MOD(r, 1, 22, 22);      /* CLKINP_DIVBY2EN = enable */
+                r = FLD_MOD(r, 1, 23, 23);      /* CLKINP_SEL = enable */
+        }
+
         dsi_write_reg(dsidev, DSI_DSIPHY_CFG1, r);
 
         r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
@@ -2147,10 +2313,9 @@ static void dsi_cio_timings(struct platform_device *dsidev)
 }
 
 /* lane masks have lane 0 at lsb. mask_p for positive lines, n for negative */
-static void dsi_cio_enable_lane_override(struct omap_dss_device *dssdev,
+static void dsi_cio_enable_lane_override(struct platform_device *dsidev,
                 unsigned mask_p, unsigned mask_n)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int i;
         u32 l;
@@ -2197,9 +2362,8 @@ static void dsi_cio_disable_lane_override(struct platform_device *dsidev)
         REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 22, 17);
 }
 
-static int dsi_cio_wait_tx_clk_esc_reset(struct omap_dss_device *dssdev)
+static int dsi_cio_wait_tx_clk_esc_reset(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int t, i;
         bool in_use[DSI_MAX_NR_LANES];
@@ -2247,9 +2411,8 @@ static int dsi_cio_wait_tx_clk_esc_reset(struct omap_dss_device *dssdev)
 }
 
 /* return bitmask of enabled lanes, lane0 being the lsb */
-static unsigned dsi_get_lane_mask(struct omap_dss_device *dssdev)
+static unsigned dsi_get_lane_mask(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         unsigned mask = 0;
         int i;
@@ -2262,16 +2425,15 @@ static unsigned dsi_get_lane_mask(struct omap_dss_device *dssdev)
         return mask;
 }
 
-static int dsi_cio_init(struct omap_dss_device *dssdev)
+static int dsi_cio_init(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int r;
         u32 l;
 
         DSSDBGF();
 
-        r = dss_dsi_enable_pads(dsi->module_id, dsi_get_lane_mask(dssdev));
+        r = dss_dsi_enable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
         if (r)
                 return r;
 
@@ -2288,7 +2450,7 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
                 goto err_scp_clk_dom;
         }
 
-        r = dsi_set_lane_config(dssdev);
+        r = dsi_set_lane_config(dsidev);
         if (r)
                 goto err_scp_clk_dom;
 
@@ -2323,7 +2485,7 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
                         mask_p |= 1 << i;
                 }
 
-                dsi_cio_enable_lane_override(dssdev, mask_p, 0);
+                dsi_cio_enable_lane_override(dsidev, mask_p, 0);
         }
 
         r = dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ON);
@@ -2340,7 +2502,7 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
         dsi_if_enable(dsidev, false);
         REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
 
-        r = dsi_cio_wait_tx_clk_esc_reset(dssdev);
+        r = dsi_cio_wait_tx_clk_esc_reset(dsidev);
         if (r)
                 goto err_tx_clk_esc_rst;
 
@@ -2360,10 +2522,10 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
 
         dsi_cio_timings(dsidev);
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
                 /* DDR_CLK_ALWAYS_ON */
                 REG_FLD_MOD(dsidev, DSI_CLK_CTRL,
-                        dssdev->panel.dsi_vm_data.ddr_clk_always_on, 13, 13);
+                        dsi->vm_timings.ddr_clk_always_on, 13, 13);
         }
 
         dsi->ulps_enabled = false;
@@ -2381,13 +2543,12 @@ err_cio_pwr:
                 dsi_cio_disable_lane_override(dsidev);
 err_scp_clk_dom:
         dsi_disable_scp_clk(dsidev);
-        dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dssdev));
+        dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
         return r;
 }
 
-static void dsi_cio_uninit(struct omap_dss_device *dssdev)
+static void dsi_cio_uninit(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
         /* DDR_CLK_ALWAYS_ON */
@@ -2395,7 +2556,7 @@ static void dsi_cio_uninit(struct omap_dss_device *dssdev)
 
         dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
         dsi_disable_scp_clk(dsidev);
-        dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dssdev));
+        dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
 }
 
 static void dsi_config_tx_fifo(struct platform_device *dsidev,
@@ -2685,6 +2846,7 @@ void omapdss_dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
                 bool enable)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
         DSSDBG("dsi_vc_enable_hs(%d, %d)\n", channel, enable);
 
@@ -2701,7 +2863,7 @@ void omapdss_dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
         dsi_force_tx_stop_mode_io(dsidev);
 
         /* start the DDR clock by sending a NULL packet */
-        if (dssdev->panel.dsi_vm_data.ddr_clk_always_on && enable)
+        if (dsi->vm_timings.ddr_clk_always_on && enable)
                 dsi_vc_send_null(dssdev, channel);
 }
 EXPORT_SYMBOL(omapdss_dsi_vc_enable_hs);
@@ -2987,10 +3149,9 @@ int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel)
 }
 EXPORT_SYMBOL(dsi_vc_send_null);
 
-static int dsi_vc_write_nosync_common(struct omap_dss_device *dssdev,
+static int dsi_vc_write_nosync_common(struct platform_device *dsidev,
                 int channel, u8 *data, int len, enum dss_dsi_content_type type)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         int r;
 
         if (len == 0) {
@@ -3021,7 +3182,9 @@ static int dsi_vc_write_nosync_common(struct omap_dss_device *dssdev,
 int dsi_vc_dcs_write_nosync(struct omap_dss_device *dssdev, int channel,
                 u8 *data, int len)
 {
-        return dsi_vc_write_nosync_common(dssdev, channel, data, len,
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+        return dsi_vc_write_nosync_common(dsidev, channel, data, len,
                         DSS_DSI_CONTENT_DCS);
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_nosync);
@@ -3029,7 +3192,9 @@ EXPORT_SYMBOL(dsi_vc_dcs_write_nosync);
 int dsi_vc_generic_write_nosync(struct omap_dss_device *dssdev, int channel,
                 u8 *data, int len)
 {
-        return dsi_vc_write_nosync_common(dssdev, channel, data, len,
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+        return dsi_vc_write_nosync_common(dsidev, channel, data, len,
                         DSS_DSI_CONTENT_GENERIC);
 }
 EXPORT_SYMBOL(dsi_vc_generic_write_nosync);
@@ -3040,7 +3205,7 @@ static int dsi_vc_write_common(struct omap_dss_device *dssdev, int channel,
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         int r;
 
-        r = dsi_vc_write_nosync_common(dssdev, channel, data, len, type);
+        r = dsi_vc_write_nosync_common(dsidev, channel, data, len, type);
         if (r)
                 goto err;
 
@@ -3118,10 +3283,9 @@ int dsi_vc_generic_write_2(struct omap_dss_device *dssdev, int channel,
 }
 EXPORT_SYMBOL(dsi_vc_generic_write_2);
 
-static int dsi_vc_dcs_send_read_request(struct omap_dss_device *dssdev,
+static int dsi_vc_dcs_send_read_request(struct platform_device *dsidev,
                 int channel, u8 dcs_cmd)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int r;
 
@@ -3139,10 +3303,9 @@ static int dsi_vc_dcs_send_read_request(struct omap_dss_device *dssdev,
         return 0;
 }
 
-static int dsi_vc_generic_send_read_request(struct omap_dss_device *dssdev,
+static int dsi_vc_generic_send_read_request(struct platform_device *dsidev,
                 int channel, u8 *reqdata, int reqlen)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u16 data;
         u8 data_type;
@@ -3291,7 +3454,7 @@ int dsi_vc_dcs_read(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         int r;
 
-        r = dsi_vc_dcs_send_read_request(dssdev, channel, dcs_cmd);
+        r = dsi_vc_dcs_send_read_request(dsidev, channel, dcs_cmd);
         if (r)
                 goto err;
 
@@ -3322,7 +3485,7 @@ static int dsi_vc_generic_read(struct omap_dss_device *dssdev, int channel,
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         int r;
 
-        r = dsi_vc_generic_send_read_request(dssdev, channel, reqdata, reqlen);
+        r = dsi_vc_generic_send_read_request(dsidev, channel, reqdata, reqlen);
         if (r)
                 return r;
 
@@ -3604,15 +3767,15 @@ static void dsi_set_hs_tx_timeout(struct platform_device *dsidev,
                         (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_config_vp_num_line_buffers(struct omap_dss_device *dssdev)
+static void dsi_config_vp_num_line_buffers(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int num_line_buffers;
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
-                int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                int bpp = dsi_get_pixel_size(dsi->pix_fmt);
                 unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
-                struct omap_video_timings *timings = &dssdev->panel.timings;
+                struct omap_video_timings *timings = &dsi->timings;
                 /*
                  * Don't use line buffers if width is greater than the video
                  * port's line buffer size
@@ -3630,11 +3793,11 @@ static void dsi_config_vp_num_line_buffers(struct omap_dss_device *dssdev)
         REG_FLD_MOD(dsidev, DSI_CTRL, num_line_buffers, 13, 12);
 }
 
-static void dsi_config_vp_sync_events(struct omap_dss_device *dssdev)
+static void dsi_config_vp_sync_events(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-        bool vsync_end = dssdev->panel.dsi_vm_data.vp_vsync_end;
-        bool hsync_end = dssdev->panel.dsi_vm_data.vp_hsync_end;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        bool vsync_end = dsi->vm_timings.vp_vsync_end;
+        bool hsync_end = dsi->vm_timings.vp_hsync_end;
         u32 r;
 
         r = dsi_read_reg(dsidev, DSI_CTRL);
@@ -3648,13 +3811,13 @@ static void dsi_config_vp_sync_events(struct omap_dss_device *dssdev)
         dsi_write_reg(dsidev, DSI_CTRL, r);
 }
 
-static void dsi_config_blanking_modes(struct omap_dss_device *dssdev)
+static void dsi_config_blanking_modes(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-        int blanking_mode = dssdev->panel.dsi_vm_data.blanking_mode;
-        int hfp_blanking_mode = dssdev->panel.dsi_vm_data.hfp_blanking_mode;
-        int hbp_blanking_mode = dssdev->panel.dsi_vm_data.hbp_blanking_mode;
-        int hsa_blanking_mode = dssdev->panel.dsi_vm_data.hsa_blanking_mode;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int blanking_mode = dsi->vm_timings.blanking_mode;
+        int hfp_blanking_mode = dsi->vm_timings.hfp_blanking_mode;
+        int hbp_blanking_mode = dsi->vm_timings.hbp_blanking_mode;
+        int hsa_blanking_mode = dsi->vm_timings.hsa_blanking_mode;
         u32 r;
 
         /*
@@ -3741,8 +3904,8 @@ static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
         int ddr_clk_pre, ddr_clk_post, enter_hs_mode_lat, exit_hs_mode_lat;
         int tclk_trail, ths_exit, exiths_clk;
         bool ddr_alwon;
-        struct omap_video_timings *timings = &dssdev->panel.timings;
-        int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+        struct omap_video_timings *timings = &dsi->timings;
+        int bpp = dsi_get_pixel_size(dsi->pix_fmt);
         int ndl = dsi->num_lanes_used - 1;
         int dsi_fclk_hsdiv = dssdev->clocks.dsi.regm_dsi + 1;
         int hsa_interleave_hs = 0, hsa_interleave_lp = 0;
@@ -3852,6 +4015,7 @@ static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
 static int dsi_proto_config(struct omap_dss_device *dssdev)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u32 r;
         int buswidth = 0;
 
@@ -3871,7 +4035,7 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
         dsi_set_lp_rx_timeout(dsidev, 0x1fff, true, true);
         dsi_set_hs_tx_timeout(dsidev, 0x1fff, true, true);
 
-        switch (dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt)) {
+        switch (dsi_get_pixel_size(dsi->pix_fmt)) {
         case 16:
                 buswidth = 0;
                 break;
@@ -3903,11 +4067,11 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
 
         dsi_write_reg(dsidev, DSI_CTRL, r);
 
-        dsi_config_vp_num_line_buffers(dssdev);
+        dsi_config_vp_num_line_buffers(dsidev);
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
-                dsi_config_vp_sync_events(dssdev);
-                dsi_config_blanking_modes(dssdev);
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                dsi_config_vp_sync_events(dsidev);
+                dsi_config_blanking_modes(dsidev);
                 dsi_config_cmd_mode_interleaving(dssdev);
         }
 
@@ -3919,9 +4083,8 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
         return 0;
 }
 
-static void dsi_proto_timings(struct omap_dss_device *dssdev)
+static void dsi_proto_timings(struct platform_device *dsidev)
 {
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         unsigned tlpx, tclk_zero, tclk_prepare, tclk_trail;
         unsigned tclk_pre, tclk_post;
@@ -3941,7 +4104,7 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
         ths_exit = FLD_GET(r, 7, 0);
 
         r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
-        tlpx = FLD_GET(r, 22, 16) * 2;
+        tlpx = FLD_GET(r, 20, 16) * 2;
         tclk_trail = FLD_GET(r, 15, 8);
         tclk_zero = FLD_GET(r, 7, 0);
 
@@ -3984,18 +4147,18 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
         DSSDBG("enter_hs_mode_lat %u, exit_hs_mode_lat %u\n",
                         enter_hs_mode_lat, exit_hs_mode_lat);
 
-         if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+         if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
                 /* TODO: Implement a video mode check_timings function */
-                int hsa = dssdev->panel.dsi_vm_data.hsa;
-                int hfp = dssdev->panel.dsi_vm_data.hfp;
-                int hbp = dssdev->panel.dsi_vm_data.hbp;
-                int vsa = dssdev->panel.dsi_vm_data.vsa;
-                int vfp = dssdev->panel.dsi_vm_data.vfp;
-                int vbp = dssdev->panel.dsi_vm_data.vbp;
-                int window_sync = dssdev->panel.dsi_vm_data.window_sync;
-                bool hsync_end = dssdev->panel.dsi_vm_data.vp_hsync_end;
-                struct omap_video_timings *timings = &dssdev->panel.timings;
-                int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+                int hsa = dsi->vm_timings.hsa;
+                int hfp = dsi->vm_timings.hfp;
+                int hbp = dsi->vm_timings.hbp;
+                int vsa = dsi->vm_timings.vsa;
+                int vfp = dsi->vm_timings.vfp;
+                int vbp = dsi->vm_timings.vbp;
+                int window_sync = dsi->vm_timings.window_sync;
+                bool hsync_end = dsi->vm_timings.vp_hsync_end;
+                struct omap_video_timings *timings = &dsi->timings;
+                int bpp = dsi_get_pixel_size(dsi->pix_fmt);
                 int tl, t_he, width_bytes;
 
                 t_he = hsync_end ?
@@ -4100,16 +4263,84 @@ int omapdss_dsi_configure_pins(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(omapdss_dsi_configure_pins);
 
-int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
+int omapdss_dsi_set_clocks(struct omap_dss_device *dssdev,
+                unsigned long ddr_clk, unsigned long lp_clk)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dsi_clock_info cinfo;
+        struct dispc_clock_info dispc_cinfo;
+        unsigned lp_clk_div;
+        unsigned long dsi_fclk;
         int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+        unsigned long pck;
+        int r;
+
+        DSSDBGF("ddr_clk %lu, lp_clk %lu", ddr_clk, lp_clk);
+
+        mutex_lock(&dsi->lock);
+
+        /* Calculate PLL output clock */
+        r = dsi_pll_calc_ddrfreq(dsidev, ddr_clk * 4, &cinfo);
+        if (r)
+                goto err;
+
+        /* Calculate PLL's DSI clock */
+        dsi_pll_calc_dsi_fck(dsidev, &cinfo);
+
+        /* Calculate PLL's DISPC clock and pck & lck divs */
+        pck = cinfo.clkin4ddr / 16 * (dsi->num_lanes_used - 1) * 8 / bpp;
+        DSSDBG("finding dispc dividers for pck %lu\n", pck);
+        r = dsi_pll_calc_dispc_fck(dsidev, pck, &cinfo, &dispc_cinfo);
+        if (r)
+                goto err;
+
+        /* Calculate LP clock */
+        dsi_fclk = cinfo.dsi_pll_hsdiv_dsi_clk;
+        lp_clk_div = DIV_ROUND_UP(dsi_fclk, lp_clk * 2);
+
+        dssdev->clocks.dsi.regn = cinfo.regn;
+        dssdev->clocks.dsi.regm = cinfo.regm;
+        dssdev->clocks.dsi.regm_dispc = cinfo.regm_dispc;
+        dssdev->clocks.dsi.regm_dsi = cinfo.regm_dsi;
+
+        dssdev->clocks.dsi.lp_clk_div = lp_clk_div;
+
+        dssdev->clocks.dispc.channel.lck_div = dispc_cinfo.lck_div;
+        dssdev->clocks.dispc.channel.pck_div = dispc_cinfo.pck_div;
+
+        dssdev->clocks.dispc.dispc_fclk_src = OMAP_DSS_CLK_SRC_FCK;
+
+        dssdev->clocks.dispc.channel.lcd_clk_src =
+                dsi->module_id == 0 ?
+                OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+                OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+
+        dssdev->clocks.dsi.dsi_fclk_src =
+                dsi->module_id == 0 ?
+                OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+                OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI;
+
+        mutex_unlock(&dsi->lock);
+        return 0;
+err:
+        mutex_unlock(&dsi->lock);
+        return r;
+}
+EXPORT_SYMBOL(omapdss_dsi_set_clocks);
+
+int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+        int bpp = dsi_get_pixel_size(dsi->pix_fmt);
         u8 data_type;
         u16 word_count;
         int r;
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
-                switch (dssdev->panel.dsi_pix_fmt) {
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                switch (dsi->pix_fmt) {
                 case OMAP_DSS_DSI_FMT_RGB888:
                         data_type = MIPI_DSI_PACKED_PIXEL_STREAM_24;
                         break;
@@ -4133,7 +4364,7 @@ int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
                 /* MODE, 1 = video mode */
                 REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 4, 4);
 
-                word_count = DIV_ROUND_UP(dssdev->panel.timings.x_res * bpp, 8);
+                word_count = DIV_ROUND_UP(dsi->timings.x_res * bpp, 8);
 
                 dsi_vc_write_long_header(dsidev, channel, data_type,
                                 word_count, 0);
@@ -4142,9 +4373,9 @@ int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
                 dsi_if_enable(dsidev, true);
         }
 
-        r = dss_mgr_enable(dssdev->manager);
+        r = dss_mgr_enable(mgr);
         if (r) {
-                if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
                         dsi_if_enable(dsidev, false);
                         dsi_vc_enable(dsidev, channel, false);
                 }
@@ -4159,8 +4390,10 @@ EXPORT_SYMBOL(dsi_enable_video_output);
 void dsi_disable_video_output(struct omap_dss_device *dssdev, int channel)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
                 dsi_if_enable(dsidev, false);
                 dsi_vc_enable(dsidev, channel, false);
 
@@ -4171,15 +4404,15 @@ void dsi_disable_video_output(struct omap_dss_device *dssdev, int channel)
                 dsi_if_enable(dsidev, true);
         }
 
-        dss_mgr_disable(dssdev->manager);
+        dss_mgr_disable(mgr);
 }
 EXPORT_SYMBOL(dsi_disable_video_output);
 
-static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
-                u16 w, u16 h)
+static void dsi_update_screen_dispc(struct omap_dss_device *dssdev)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         unsigned bytespp;
         unsigned bytespl;
         unsigned bytespf;
@@ -4190,12 +4423,14 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
         int r;
         const unsigned channel = dsi->update_channel;
         const unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
+        u16 w = dsi->timings.x_res;
+        u16 h = dsi->timings.y_res;
 
         DSSDBG("dsi_update_screen_dispc(%dx%d)\n", w, h);
 
         dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_VP);
 
-        bytespp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt) / 8;
+        bytespp = dsi_get_pixel_size(dsi->pix_fmt) / 8;
         bytespl = w * bytespp;
         bytespf = bytespl * h;
 
@@ -4239,7 +4474,9 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
                 msecs_to_jiffies(250));
         BUG_ON(r == 0);
 
-        dss_mgr_start_update(dssdev->manager);
+        dss_mgr_set_timings(mgr, &dsi->timings);
+
+        dss_mgr_start_update(mgr);
 
         if (dsi->te_enabled) {
                 /* disable LP_RX_TO, so that we can receive TE.  Time to wait
@@ -4297,8 +4534,7 @@ static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 
 static void dsi_framedone_irq_callback(void *data, u32 mask)
 {
-        struct omap_dss_device *dssdev = (struct omap_dss_device *) data;
-        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct platform_device *dsidev = (struct platform_device *) data;
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
         /* Note: We get FRAMEDONE when DISPC has finished sending pixels and
@@ -4325,13 +4561,14 @@ int omap_dsi_update(struct omap_dss_device *dssdev, int channel,
         dsi->framedone_callback = callback;
         dsi->framedone_data = data;
 
-        dssdev->driver->get_resolution(dssdev, &dw, &dh);
+        dw = dsi->timings.x_res;
+        dh = dsi->timings.y_res;
 
 #ifdef DEBUG
         dsi->update_bytes = dw * dh *
-                dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt) / 8;
+                dsi_get_pixel_size(dsi->pix_fmt) / 8;
 #endif
-        dsi_update_screen_dispc(dssdev, dw, dh);
+        dsi_update_screen_dispc(dssdev);
 
         return 0;
 }
@@ -4367,28 +4604,22 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-        struct omap_video_timings timings;
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         int r;
         u32 irq = 0;
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
-                u16 dw, dh;
-
-                dssdev->driver->get_resolution(dssdev, &dw, &dh);
-
-                timings.x_res = dw;
-                timings.y_res = dh;
-                timings.hsw = 1;
-                timings.hfp = 1;
-                timings.hbp = 1;
-                timings.vsw = 1;
-                timings.vfp = 0;
-                timings.vbp = 0;
+        if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
+                dsi->timings.hsw = 1;
+                dsi->timings.hfp = 1;
+                dsi->timings.hbp = 1;
+                dsi->timings.vsw = 1;
+                dsi->timings.vfp = 0;
+                dsi->timings.vbp = 0;
 
-                irq = dispc_mgr_get_framedone_irq(dssdev->manager->id);
+                irq = dispc_mgr_get_framedone_irq(mgr->id);
 
                 r = omap_dispc_register_isr(dsi_framedone_irq_callback,
-                        (void *) dssdev, irq);
+                        (void *) dsidev, irq);
                 if (r) {
                         DSSERR("can't get FRAMEDONE irq\n");
                         goto err;
@@ -4397,8 +4628,6 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
                 dsi->mgr_config.stallmode = true;
                 dsi->mgr_config.fifohandcheck = true;
         } else {
-                timings = dssdev->panel.timings;
-
                 dsi->mgr_config.stallmode = false;
                 dsi->mgr_config.fifohandcheck = false;
         }
@@ -4407,14 +4636,14 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
          * override interlace, logic level and edge related parameters in
          * omap_video_timings with default values
          */
-        timings.interlace = false;
-        timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
-        timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
-        timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
-        timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
-        timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+        dsi->timings.interlace = false;
+        dsi->timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        dsi->timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        dsi->timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        dsi->timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        dsi->timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
 
-        dss_mgr_set_timings(dssdev->manager, &timings);
+        dss_mgr_set_timings(mgr, &dsi->timings);
 
         r = dsi_configure_dispc_clocks(dssdev);
         if (r)
@@ -4422,29 +4651,33 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 
         dsi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
         dsi->mgr_config.video_port_width =
-                        dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+                        dsi_get_pixel_size(dsi->pix_fmt);
         dsi->mgr_config.lcden_sig_polarity = 0;
 
-        dss_mgr_set_lcd_config(dssdev->manager, &dsi->mgr_config);
+        dss_mgr_set_lcd_config(mgr, &dsi->mgr_config);
 
         return 0;
 err1:
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE)
+        if (dsi->mode == OMAP_DSS_DSI_CMD_MODE)
                 omap_dispc_unregister_isr(dsi_framedone_irq_callback,
-                        (void *) dssdev, irq);
+                        (void *) dsidev, irq);
 err:
         return r;
 }
 
 static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
 {
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+        if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
                 u32 irq;
 
-                irq = dispc_mgr_get_framedone_irq(dssdev->manager->id);
+                irq = dispc_mgr_get_framedone_irq(mgr->id);
 
                 omap_dispc_unregister_isr(dsi_framedone_irq_callback,
-                        (void *) dssdev, irq);
+                        (void *) dsidev, irq);
         }
 }
 
@@ -4477,6 +4710,7 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         int r;
 
         r = dsi_pll_init(dsidev, true, true);
@@ -4489,18 +4723,18 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 
         dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
         dss_select_dsi_clk_source(dsi->module_id, dssdev->clocks.dsi.dsi_fclk_src);
-        dss_select_lcd_clk_source(dssdev->manager->id,
+        dss_select_lcd_clk_source(mgr->id,
                         dssdev->clocks.dispc.channel.lcd_clk_src);
 
         DSSDBG("PLL OK\n");
 
-        r = dsi_cio_init(dssdev);
+        r = dsi_cio_init(dsidev);
         if (r)
                 goto err2;
 
         _dsi_print_reset_status(dsidev);
 
-        dsi_proto_timings(dssdev);
+        dsi_proto_timings(dsidev);
         dsi_set_lp_clk_divisor(dssdev);
 
         if (1)
@@ -4520,11 +4754,11 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 
         return 0;
 err3:
-        dsi_cio_uninit(dssdev);
+        dsi_cio_uninit(dsidev);
 err2:
         dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
         dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
-        dss_select_lcd_clk_source(dssdev->manager->id, OMAP_DSS_CLK_SRC_FCK);
+        dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
 
 err1:
         dsi_pll_uninit(dsidev, true);
@@ -4537,6 +4771,7 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
 
         if (enter_ulps && !dsi->ulps_enabled)
                 dsi_enter_ulps(dsidev);
@@ -4550,8 +4785,8 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
 
         dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
         dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
-        dss_select_lcd_clk_source(dssdev->manager->id, OMAP_DSS_CLK_SRC_FCK);
-        dsi_cio_uninit(dssdev);
+        dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+        dsi_cio_uninit(dsidev);
         dsi_pll_uninit(dsidev, disconnect_lanes);
 }
 
@@ -4559,6 +4794,7 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_dss_output *out = dssdev->output;
         int r = 0;
 
         DSSDBG("dsi_display_enable\n");
@@ -4567,8 +4803,8 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 
         mutex_lock(&dsi->lock);
 
-        if (dssdev->manager == NULL) {
-                DSSERR("failed to enable display: no manager\n");
+        if (out == NULL || out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
                 r = -ENODEV;
                 goto err_start_dev;
         }
@@ -4653,17 +4889,83 @@ int omapdss_dsi_enable_te(struct omap_dss_device *dssdev, bool enable)
 }
 EXPORT_SYMBOL(omapdss_dsi_enable_te);
 
-static int __init dsi_init_display(struct omap_dss_device *dssdev)
+void omapdss_dsi_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
 {
         struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
-        DSSDBG("DSI init\n");
+        mutex_lock(&dsi->lock);
 
-        if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
-                dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
-                        OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
-        }
+        dsi->timings = *timings;
+
+        mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_timings);
+
+void omapdss_dsi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        mutex_lock(&dsi->lock);
+
+        dsi->timings.x_res = w;
+        dsi->timings.y_res = h;
+
+        mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_size);
+
+void omapdss_dsi_set_pixel_format(struct omap_dss_device *dssdev,
+                enum omap_dss_dsi_pixel_format fmt)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        mutex_lock(&dsi->lock);
+
+        dsi->pix_fmt = fmt;
+
+        mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_pixel_format);
+
+void omapdss_dsi_set_operation_mode(struct omap_dss_device *dssdev,
+                enum omap_dss_dsi_mode mode)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        mutex_lock(&dsi->lock);
+
+        dsi->mode = mode;
+
+        mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_operation_mode);
+
+void omapdss_dsi_set_videomode_timings(struct omap_dss_device *dssdev,
+                struct omap_dss_dsi_videomode_timings *timings)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        mutex_lock(&dsi->lock);
+
+        dsi->vm_timings = *timings;
+
+        mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_videomode_timings);
+
+static int __init dsi_init_display(struct omap_dss_device *dssdev)
+{
+        struct platform_device *dsidev =
+                        dsi_get_dsidev_from_id(dssdev->phy.dsi.module);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        DSSDBG("DSI init\n");
 
         if (dsi->vdds_dsi_reg == NULL) {
                 struct regulator *vdds_dsi;
@@ -4806,11 +5108,15 @@ static void dsi_put_clocks(struct platform_device *dsidev)
                 clk_put(dsi->sys_clk);
 }
 
-static void __init dsi_probe_pdata(struct platform_device *dsidev)
+static struct omap_dss_device * __init dsi_find_dssdev(struct platform_device *pdev)
 {
-        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-        struct omap_dss_board_info *pdata = dsidev->dev.platform_data;
-        int i, r;
+        struct omap_dss_board_info *pdata = pdev->dev.platform_data;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+        const char *def_disp_name = dss_get_default_display_name();
+        struct omap_dss_device *def_dssdev;
+        int i;
+
+        def_dssdev = NULL;
 
         for (i = 0; i < pdata->num_devices; ++i) {
                 struct omap_dss_device *dssdev = pdata->devices[i];
@@ -4821,19 +5127,73 @@ static void __init dsi_probe_pdata(struct platform_device *dsidev)
                 if (dssdev->phy.dsi.module != dsi->module_id)
                         continue;
 
-                r = dsi_init_display(dssdev);
-                if (r) {
-                        DSSERR("device %s init failed: %d\n", dssdev->name, r);
-                        continue;
+                if (def_dssdev == NULL)
+                        def_dssdev = dssdev;
+
+                if (def_disp_name != NULL &&
+                                strcmp(dssdev->name, def_disp_name) == 0) {
+                        def_dssdev = dssdev;
+                        break;
                 }
+        }
 
-                r = omap_dss_register_device(dssdev, &dsidev->dev, i);
-                if (r)
-                        DSSERR("device %s register failed: %d\n",
-                                        dssdev->name, r);
+        return def_dssdev;
+}
+
+static void __init dsi_probe_pdata(struct platform_device *dsidev)
+{
+        struct omap_dss_device *plat_dssdev;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        plat_dssdev = dsi_find_dssdev(dsidev);
+
+        if (!plat_dssdev)
+                return;
+
+        dssdev = dss_alloc_and_init_device(&dsidev->dev);
+        if (!dssdev)
+                return;
+
+        dss_copy_device_pdata(dssdev, plat_dssdev);
+
+        r = dsi_init_display(dssdev);
+        if (r) {
+                DSSERR("device %s init failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
+        }
+
+        r = dss_add_device(dssdev);
+        if (r) {
+                DSSERR("device %s register failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
         }
 }
 
+static void __init dsi_init_output(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_dss_output *out = &dsi->output;
+
+        out->pdev = dsidev;
+        out->id = dsi->module_id == 0 ?
+                        OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
+
+        out->type = OMAP_DISPLAY_TYPE_DSI;
+
+        dss_register_output(out);
+}
+
+static void __exit dsi_uninit_output(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_dss_output *out = &dsi->output;
+
+        dss_unregister_output(out);
+}
+
 /* DSI1 HW IP initialisation */
 static int __init omap_dsihw_probe(struct platform_device *dsidev)
 {
@@ -4848,7 +5208,6 @@ static int __init omap_dsihw_probe(struct platform_device *dsidev)
 
         dsi->module_id = dsidev->id;
         dsi->pdev = dsidev;
-        dsi_pdev_map[dsi->module_id] = dsidev;
         dev_set_drvdata(&dsidev->dev, dsi);
 
         spin_lock_init(&dsi->irq_lock);
@@ -4928,6 +5287,8 @@ static int __init omap_dsihw_probe(struct platform_device *dsidev)
         else
                 dsi->num_lanes_supported = 3;
 
+        dsi_init_output(dsidev);
+
         dsi_probe_pdata(dsidev);
 
         dsi_runtime_put(dsidev);
@@ -4957,7 +5318,9 @@ static int __exit omap_dsihw_remove(struct platform_device *dsidev)
 
         WARN_ON(dsi->scp_clk_refcount > 0);
 
-        omap_dss_unregister_child_devices(&dsidev->dev);
+        dss_unregister_child_devices(&dsidev->dev);
+
+        dsi_uninit_output(dsidev);
 
         pm_runtime_disable(&dsidev->dev);
 
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 04b4586113e3..2ab1c3e96553 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -31,11 +31,11 @@
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/gfp.h>
 
 #include <video/omapdss.h>
 
 #include <plat/cpu.h>
-#include <plat/clock.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -65,6 +65,13 @@ struct dss_reg {
 static int dss_runtime_get(void);
 static void dss_runtime_put(void);
 
+struct dss_features {
+        u8 fck_div_max;
+        u8 dss_fck_multiplier;
+        const char *clk_name;
+        int (*dpi_select_source)(enum omap_channel channel);
+};
+
 static struct {
         struct platform_device *pdev;
         void __iomem    *base;
@@ -83,6 +90,8 @@ static struct {
 
         bool            ctx_valid;
         u32             ctx[DSS_SZ_REGS / sizeof(u32)];
+
+        const struct dss_features *feat;
 } dss;
 
 static const char * const dss_generic_clk_source_names[] = {
@@ -144,7 +153,7 @@ static void dss_restore_context(void)
 #undef SR
 #undef RR
 
-void dss_sdi_init(u8 datapairs)
+void dss_sdi_init(int datapairs)
 {
         u32 l;
 
@@ -236,7 +245,6 @@ const char *dss_get_generic_clk_source_name(enum omap_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;
@@ -259,18 +267,10 @@ void dss_dump_clocks(struct seq_file *s)
 
                 seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
 
-                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);
+                seq_printf(s, "%s (%s) = %lu / %lu * %d  = %lu\n",
+                                fclk_name, fclk_real_name, dpll4_ck_rate,
+                                dpll4_ck_rate / dpll4_m4_ck_rate,
+                                dss.feat->dss_fck_multiplier, fclk_rate);
         } else {
                 seq_printf(s, "%s (%s) = %lu\n",
                                 fclk_name, fclk_real_name,
@@ -431,31 +431,6 @@ enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
         }
 }
 
-/* calculate clock rates using dividers in cinfo */
-int dss_calc_clock_rates(struct dss_clock_info *cinfo)
-{
-        if (dss.dpll4_m4_ck) {
-                unsigned long prate;
-                u16 fck_div_max = 16;
-
-                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));
-
-                cinfo->fck = prate / cinfo->fck_div;
-        } else {
-                if (cinfo->fck_div != 0)
-                        return -EINVAL;
-                cinfo->fck = clk_get_rate(dss.dss_clk);
-        }
-
-        return 0;
-}
-
 int dss_set_clock_div(struct dss_clock_info *cinfo)
 {
         if (dss.dpll4_m4_ck) {
@@ -478,26 +453,6 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
         return 0;
 }
 
-int dss_get_clock_div(struct dss_clock_info *cinfo)
-{
-        cinfo->fck = clk_get_rate(dss.dss_clk);
-
-        if (dss.dpll4_m4_ck) {
-                unsigned long prate;
-
-                prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-
-                if (cpu_is_omap3630() || cpu_is_omap44xx())
-                        cinfo->fck_div = prate / (cinfo->fck);
-                else
-                        cinfo->fck_div = prate / (cinfo->fck / 2);
-        } else {
-                cinfo->fck_div = 0;
-        }
-
-        return 0;
-}
-
 unsigned long dss_get_dpll4_rate(void)
 {
         if (dss.dpll4_m4_ck)
@@ -515,7 +470,7 @@ int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
 
         unsigned long fck, max_dss_fck;
 
-        u16 fck_div, fck_div_max = 16;
+        u16 fck_div;
 
         int match = 0;
         int min_fck_per_pck;
@@ -525,9 +480,8 @@ int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
         max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
 
         fck = clk_get_rate(dss.dss_clk);
-        if (req_pck == dss.cache_req_pck &&
-                        ((cpu_is_omap34xx() && prate == dss.cache_prate) ||
-                         dss.cache_dss_cinfo.fck == fck)) {
+        if (req_pck == dss.cache_req_pck && prate == dss.cache_prate &&
+                dss.cache_dss_cinfo.fck == fck) {
                 DSSDBG("dispc clock info found from cache.\n");
                 *dss_cinfo = dss.cache_dss_cinfo;
                 *dispc_cinfo = dss.cache_dispc_cinfo;
@@ -564,16 +518,10 @@ retry:
 
                 goto found;
         } else {
-                if (cpu_is_omap3630() || cpu_is_omap44xx())
-                        fck_div_max = 32;
-
-                for (fck_div = fck_div_max; fck_div > 0; --fck_div) {
+                for (fck_div = dss.feat->fck_div_max; fck_div > 0; --fck_div) {
                         struct dispc_clock_info cur_dispc;
 
-                        if (fck_div_max == 32)
-                                fck = prate / fck_div;
-                        else
-                                fck = prate / fck_div * 2;
+                        fck = prate / fck_div * dss.feat->dss_fck_multiplier;
 
                         if (fck > max_dss_fck)
                                 continue;
@@ -648,9 +596,18 @@ void dss_set_dac_pwrdn_bgz(bool enable)
         REG_FLD_MOD(DSS_CONTROL, enable, 5, 5); /* DAC Power-Down Control */
 }
 
-void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select hdmi)
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select src)
 {
-        REG_FLD_MOD(DSS_CONTROL, hdmi, 15, 15); /* VENC_HDMI_SWITCH */
+        enum omap_display_type dp;
+        dp = dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_DIGIT);
+
+        /* Complain about invalid selections */
+        WARN_ON((src == DSS_VENC_TV_CLK) && !(dp & OMAP_DISPLAY_TYPE_VENC));
+        WARN_ON((src == DSS_HDMI_M_PCLK) && !(dp & OMAP_DISPLAY_TYPE_HDMI));
+
+        /* Select only if we have options */
+        if ((dp & OMAP_DISPLAY_TYPE_VENC) && (dp & OMAP_DISPLAY_TYPE_HDMI))
+                REG_FLD_MOD(DSS_CONTROL, src, 15, 15);  /* VENC_HDMI_SWITCH */
 }
 
 enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void)
@@ -661,9 +618,71 @@ enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void)
         if ((displays & OMAP_DISPLAY_TYPE_HDMI) == 0)
                 return DSS_VENC_TV_CLK;
 
+        if ((displays & OMAP_DISPLAY_TYPE_VENC) == 0)
+                return DSS_HDMI_M_PCLK;
+
         return REG_GET(DSS_CONTROL, 15, 15);
 }
 
+static int dss_dpi_select_source_omap2_omap3(enum omap_channel channel)
+{
+        if (channel != OMAP_DSS_CHANNEL_LCD)
+                return -EINVAL;
+
+        return 0;
+}
+
+static int dss_dpi_select_source_omap4(enum omap_channel channel)
+{
+        int val;
+
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD2:
+                val = 0;
+                break;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                val = 1;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        REG_FLD_MOD(DSS_CONTROL, val, 17, 17);
+
+        return 0;
+}
+
+static int dss_dpi_select_source_omap5(enum omap_channel channel)
+{
+        int val;
+
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                val = 1;
+                break;
+        case OMAP_DSS_CHANNEL_LCD2:
+                val = 2;
+                break;
+        case OMAP_DSS_CHANNEL_LCD3:
+                val = 3;
+                break;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                val = 0;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        REG_FLD_MOD(DSS_CONTROL, val, 17, 16);
+
+        return 0;
+}
+
+int dss_dpi_select_source(enum omap_channel channel)
+{
+        return dss.feat->dpi_select_source(channel);
+}
+
 static int dss_get_clocks(void)
 {
         struct clk *clk;
@@ -678,22 +697,11 @@ static int dss_get_clocks(void)
 
         dss.dss_clk = clk;
 
-        if (cpu_is_omap34xx()) {
-                clk = clk_get(NULL, "dpll4_m4_ck");
-                if (IS_ERR(clk)) {
-                        DSSERR("Failed to get dpll4_m4_ck\n");
-                        r = PTR_ERR(clk);
-                        goto err;
-                }
-        } else if (cpu_is_omap44xx()) {
-                clk = clk_get(NULL, "dpll_per_m5x2_ck");
-                if (IS_ERR(clk)) {
-                        DSSERR("Failed to get dpll_per_m5x2_ck\n");
-                        r = PTR_ERR(clk);
-                        goto err;
-                }
-        } else { /* omap24xx */
-                clk = NULL;
+        clk = clk_get(NULL, dss.feat->clk_name);
+        if (IS_ERR(clk)) {
+                DSSERR("Failed to get %s\n", dss.feat->clk_name);
+                r = PTR_ERR(clk);
+                goto err;
         }
 
         dss.dpll4_m4_ck = clk;
@@ -749,6 +757,71 @@ void dss_debug_dump_clocks(struct seq_file *s)
 }
 #endif
 
+static const struct dss_features omap24xx_dss_feats __initconst = {
+        .fck_div_max            =       16,
+        .dss_fck_multiplier     =       2,
+        .clk_name               =       NULL,
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+};
+
+static const struct dss_features omap34xx_dss_feats __initconst = {
+        .fck_div_max            =       16,
+        .dss_fck_multiplier     =       2,
+        .clk_name               =       "dpll4_m4_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+};
+
+static const struct dss_features omap3630_dss_feats __initconst = {
+        .fck_div_max            =       32,
+        .dss_fck_multiplier     =       1,
+        .clk_name               =       "dpll4_m4_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+};
+
+static const struct dss_features omap44xx_dss_feats __initconst = {
+        .fck_div_max            =       32,
+        .dss_fck_multiplier     =       1,
+        .clk_name               =       "dpll_per_m5x2_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap4,
+};
+
+static const struct dss_features omap54xx_dss_feats __initconst = {
+        .fck_div_max            =       64,
+        .dss_fck_multiplier     =       1,
+        .clk_name               =       "dpll_per_h12x2_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap5,
+};
+
+static int __init dss_init_features(struct device *dev)
+{
+        const struct dss_features *src;
+        struct dss_features *dst;
+
+        dst = devm_kzalloc(dev, sizeof(*dst), GFP_KERNEL);
+        if (!dst) {
+                dev_err(dev, "Failed to allocate local DSS Features\n");
+                return -ENOMEM;
+        }
+
+        if (cpu_is_omap24xx())
+                src = &omap24xx_dss_feats;
+        else if (cpu_is_omap34xx())
+                src = &omap34xx_dss_feats;
+        else if (cpu_is_omap3630())
+                src = &omap3630_dss_feats;
+        else if (cpu_is_omap44xx())
+                src = &omap44xx_dss_feats;
+        else if (soc_is_omap54xx())
+                src = &omap54xx_dss_feats;
+        else
+                return -ENODEV;
+
+        memcpy(dst, src, sizeof(*dst));
+        dss.feat = dst;
+
+        return 0;
+}
+
 /* DSS HW IP initialisation */
 static int __init omap_dsshw_probe(struct platform_device *pdev)
 {
@@ -758,6 +831,10 @@ static int __init omap_dsshw_probe(struct platform_device *pdev)
 
         dss.pdev = pdev;
 
+        r = dss_init_features(&dss.pdev->dev);
+        if (r)
+                return r;
+
         dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
         if (!dss_mem) {
                 DSSERR("can't get IORESOURCE_MEM DSS\n");
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index f67afe76f217..6728892f9dad 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -113,6 +113,17 @@ enum dss_dsi_content_type {
         DSS_DSI_CONTENT_GENERIC,
 };
 
+enum dss_writeback_channel {
+        DSS_WB_LCD1_MGR =       0,
+        DSS_WB_LCD2_MGR =       1,
+        DSS_WB_TV_MGR =         2,
+        DSS_WB_OVL0 =           3,
+        DSS_WB_OVL1 =           4,
+        DSS_WB_OVL2 =           5,
+        DSS_WB_OVL3 =           6,
+        DSS_WB_LCD3_MGR =       7,
+};
+
 struct dss_clock_info {
         /* rates that we get with dividers below */
         unsigned long fck;
@@ -175,6 +186,7 @@ struct seq_file;
 struct platform_device;
 
 /* core */
+const char *dss_get_default_display_name(void);
 struct bus_type *dss_get_bus(void);
 struct regulator *dss_get_vdds_dsi(void);
 struct regulator *dss_get_vdds_sdi(void);
@@ -184,10 +196,13 @@ void dss_dsi_disable_pads(int dsi_id, unsigned lane_mask);
 int dss_set_min_bus_tput(struct device *dev, unsigned long tput);
 int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *));
 
-int omap_dss_register_device(struct omap_dss_device *dssdev,
-                struct device *parent, int disp_num);
-void omap_dss_unregister_device(struct omap_dss_device *dssdev);
-void omap_dss_unregister_child_devices(struct device *parent);
+struct omap_dss_device *dss_alloc_and_init_device(struct device *parent);
+int dss_add_device(struct omap_dss_device *dssdev);
+void dss_unregister_device(struct omap_dss_device *dssdev);
+void dss_unregister_child_devices(struct device *parent);
+void dss_put_device(struct omap_dss_device *dssdev);
+void dss_copy_device_pdata(struct omap_dss_device *dst,
+                const struct omap_dss_device *src);
 
 /* apply */
 void dss_apply_init(void);
@@ -205,8 +220,11 @@ void dss_mgr_get_info(struct omap_overlay_manager *mgr,
 int dss_mgr_set_device(struct omap_overlay_manager *mgr,
                 struct omap_dss_device *dssdev);
 int dss_mgr_unset_device(struct omap_overlay_manager *mgr);
+int dss_mgr_set_output(struct omap_overlay_manager *mgr,
+                struct omap_dss_output *output);
+int dss_mgr_unset_output(struct omap_overlay_manager *mgr);
 void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
-                struct omap_video_timings *timings);
+                const struct omap_video_timings *timings);
 void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
                 const struct dss_lcd_mgr_config *config);
 const struct omap_video_timings *dss_mgr_get_timings(struct omap_overlay_manager *mgr);
@@ -222,12 +240,17 @@ int dss_ovl_set_manager(struct omap_overlay *ovl,
                 struct omap_overlay_manager *mgr);
 int dss_ovl_unset_manager(struct omap_overlay *ovl);
 
+/* output */
+void dss_register_output(struct omap_dss_output *out);
+void dss_unregister_output(struct omap_dss_output *out);
+struct omap_dss_output *omapdss_get_output_from_dssdev(struct omap_dss_device *dssdev);
+
 /* display */
 int dss_suspend_all_devices(void);
 int dss_resume_all_devices(void);
 void dss_disable_all_devices(void);
 
-void dss_init_device(struct platform_device *pdev,
+int dss_init_device(struct platform_device *pdev,
                 struct omap_dss_device *dssdev);
 void dss_uninit_device(struct platform_device *pdev,
                 struct omap_dss_device *dssdev);
@@ -254,22 +277,29 @@ static inline bool dss_mgr_is_lcd(enum omap_channel id)
                 return false;
 }
 
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+                struct platform_device *pdev);
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr);
+
 /* overlay */
 void dss_init_overlays(struct platform_device *pdev);
 void dss_uninit_overlays(struct platform_device *pdev);
 void dss_overlay_setup_dispc_manager(struct omap_overlay_manager *mgr);
-void dss_recheck_connections(struct omap_dss_device *dssdev, bool force);
 int dss_ovl_simple_check(struct omap_overlay *ovl,
                 const struct omap_overlay_info *info);
 int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
                 const struct omap_video_timings *mgr_timings);
 bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
                 enum omap_color_mode mode);
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+                struct platform_device *pdev);
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl);
 
 /* DSS */
 int dss_init_platform_driver(void) __init;
 void dss_uninit_platform_driver(void);
 
+int dss_dpi_select_source(enum omap_channel channel);
 void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
 enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void);
 const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src);
@@ -279,7 +309,7 @@ void dss_dump_clocks(struct seq_file *s);
 void dss_debug_dump_clocks(struct seq_file *s);
 #endif
 
-void dss_sdi_init(u8 datapairs);
+void dss_sdi_init(int datapairs);
 int dss_sdi_enable(void);
 void dss_sdi_disable(void);
 
@@ -296,9 +326,7 @@ void dss_set_venc_output(enum omap_dss_venc_type type);
 void dss_set_dac_pwrdn_bgz(bool enable);
 
 unsigned long dss_get_dpll4_rate(void);
-int dss_calc_clock_rates(struct dss_clock_info *cinfo);
 int dss_set_clock_div(struct dss_clock_info *cinfo);
-int dss_get_clock_div(struct dss_clock_info *cinfo);
 int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
                 struct dispc_clock_info *dispc_cinfo);
 
@@ -427,8 +455,9 @@ void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high);
 void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
                 u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
                 bool manual_update);
-int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
-                bool replication, const struct omap_video_timings *mgr_timings);
+int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
+                bool replication, const struct omap_video_timings *mgr_timings,
+                bool mem_to_mem);
 int dispc_ovl_enable(enum omap_plane plane, bool enable);
 void dispc_ovl_set_channel_out(enum omap_plane plane,
                 enum omap_channel channel);
@@ -457,6 +486,15 @@ int dispc_mgr_get_clock_div(enum omap_channel channel,
 void dispc_mgr_setup(enum omap_channel channel,
                 struct omap_overlay_manager_info *info);
 
+u32 dispc_wb_get_framedone_irq(void);
+bool dispc_wb_go_busy(void);
+void dispc_wb_go(void);
+void dispc_wb_enable(bool enable);
+bool dispc_wb_is_enabled(void);
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel);
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+                bool mem_to_mem, const struct omap_video_timings *timings);
+
 /* VENC */
 #ifdef CONFIG_OMAP2_DSS_VENC
 int venc_init_platform_driver(void) __init;
@@ -469,6 +507,20 @@ static inline unsigned long venc_get_pixel_clock(void)
         return 0;
 }
 #endif
+int omapdss_venc_display_enable(struct omap_dss_device *dssdev);
+void omapdss_venc_display_disable(struct omap_dss_device *dssdev);
+void omapdss_venc_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings);
+int omapdss_venc_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings);
+u32 omapdss_venc_get_wss(struct omap_dss_device *dssdev);
+int omapdss_venc_set_wss(struct omap_dss_device *dssdev, u32 wss);
+void omapdss_venc_set_type(struct omap_dss_device *dssdev,
+                enum omap_dss_venc_type type);
+void omapdss_venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
+                bool invert_polarity);
+int venc_panel_init(void);
+void venc_panel_exit(void);
 
 /* HDMI */
 #ifdef CONFIG_OMAP4_DSS_HDMI
@@ -484,7 +536,8 @@ static inline unsigned long hdmi_get_pixel_clock(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);
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings);
 int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
                                         struct omap_video_timings *timings);
 int omapdss_hdmi_read_edid(u8 *buf, int len);
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index 938709724f0c..acbc1e1efba3 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -46,7 +46,9 @@ struct omap_dss_features {
 
         const int num_mgrs;
         const int num_ovls;
+        const int num_wbs;
         const enum omap_display_type *supported_displays;
+        const enum omap_dss_output_id *supported_outputs;
         const enum omap_color_mode *supported_color_modes;
         const enum omap_overlay_caps *overlay_caps;
         const char * const *clksrc_names;
@@ -106,6 +108,21 @@ static const struct dss_reg_field omap4_dss_reg_fields[] = {
         [FEAT_REG_DSIPLL_REGM_DSI]              = { 30, 26 },
 };
 
+static const struct dss_reg_field omap5_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, 7 },
+        [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,
@@ -144,6 +161,76 @@ static const enum omap_display_type omap4_dss_supported_displays[] = {
         OMAP_DISPLAY_TYPE_DSI,
 };
 
+static const enum omap_display_type omap5_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_DSI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_HDMI | OMAP_DISPLAY_TYPE_DPI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_DSI,
+};
+
+static const enum omap_dss_output_id omap2_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3430_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_SDI | OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3630_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap4_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC | OMAP_DSS_OUTPUT_HDMI |
+        OMAP_DSS_OUTPUT_DPI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI2,
+};
+
+static const enum omap_dss_output_id omap5_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1 | OMAP_DSS_OUTPUT_DSI2,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_HDMI | OMAP_DSS_OUTPUT_DPI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_LCD3 */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI2,
+};
+
 static const enum omap_color_mode omap2_dss_supported_color_modes[] = {
         /* OMAP_DSS_GFX */
         OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
@@ -224,58 +311,80 @@ static const enum omap_color_mode omap4_dss_supported_color_modes[] = {
         OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
         OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
         OMAP_DSS_COLOR_RGBX32,
+
+        /* OMAP_DSS_WB */
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+        OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+        OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+        OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+        OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+        OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+        OMAP_DSS_COLOR_RGBX32,
 };
 
 static const enum omap_overlay_caps omap2_dss_overlay_caps[] = {
         /* OMAP_DSS_GFX */
-        0,
+        OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO1 */
-        OMAP_DSS_OVL_CAP_SCALE,
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO2 */
-        OMAP_DSS_OVL_CAP_SCALE,
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const enum omap_overlay_caps omap3430_dss_overlay_caps[] = {
         /* OMAP_DSS_GFX */
-        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA,
+        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO1 */
-        OMAP_DSS_OVL_CAP_SCALE,
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO2 */
-        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA,
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const enum omap_overlay_caps omap3630_dss_overlay_caps[] = {
         /* OMAP_DSS_GFX */
-        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA,
+        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO1 */
-        OMAP_DSS_OVL_CAP_SCALE,
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO2 */
         OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA,
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const enum omap_overlay_caps omap4_dss_overlay_caps[] = {
         /* OMAP_DSS_GFX */
         OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
-                OMAP_DSS_OVL_CAP_ZORDER,
+                OMAP_DSS_OVL_CAP_ZORDER | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO1 */
         OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER,
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO2 */
         OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER,
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
         /* OMAP_DSS_VIDEO3 */
         OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER,
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const char * const omap2_dss_clk_source_names[] = {
@@ -298,6 +407,14 @@ static const char * const omap4_dss_clk_source_names[] = {
         [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]   = "PLL2_CLK2",
 };
 
+static const char * const omap5_dss_clk_source_names[] = {
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]  = "DPLL_DSI1_A_CLK1",
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]    = "DPLL_DSI1_A_CLK2",
+        [OMAP_DSS_CLK_SRC_FCK]                  = "DSS_CLK",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC] = "DPLL_DSI1_C_CLK1",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]   = "DPLL_DSI1_C_CLK2",
+};
+
 static const struct dss_param_range omap2_dss_param_range[] = {
         [FEAT_PARAM_DSS_FCK]                    = { 0, 173000000 },
         [FEAT_PARAM_DSS_PCD]                    = { 2, 255 },
@@ -326,6 +443,7 @@ static const struct dss_param_range omap3_dss_param_range[] = {
         [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, (1 << 4) - 1 },
         [FEAT_PARAM_DSIPLL_FINT]                = { 750000, 2100000 },
         [FEAT_PARAM_DSIPLL_LPDIV]               = { 1, (1 << 13) - 1},
+        [FEAT_PARAM_DSI_FCK]                    = { 0, 173000000 },
         [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
         [FEAT_PARAM_LINEWIDTH]                  = { 1, 1024 },
         [FEAT_PARAM_MGR_WIDTH]                  = { 1, 2048 },
@@ -341,6 +459,23 @@ static const struct dss_param_range omap4_dss_param_range[] = {
         [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, (1 << 5) - 1 },
         [FEAT_PARAM_DSIPLL_FINT]                = { 500000, 2500000 },
         [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, (1 << 13) - 1 },
+        [FEAT_PARAM_DSI_FCK]                    = { 0, 170000000 },
+        [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
+        [FEAT_PARAM_LINEWIDTH]                  = { 1, 2048 },
+        [FEAT_PARAM_MGR_WIDTH]                  = { 1, 2048 },
+        [FEAT_PARAM_MGR_HEIGHT]                 = { 1, 2048 },
+};
+
+static const struct dss_param_range omap5_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 200000000 },
+        [FEAT_PARAM_DSS_PCD]                    = { 1, 255 },
+        [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 },
+        [FEAT_PARAM_DSI_FCK]                    = { 0, 170000000 },
         [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
         [FEAT_PARAM_LINEWIDTH]                  = { 1, 2048 },
         [FEAT_PARAM_MGR_WIDTH]                  = { 1, 2048 },
@@ -373,6 +508,26 @@ static const enum dss_feat_id omap3430_dss_feat_list[] = {
         FEAT_ALPHA_FIXED_ZORDER,
         FEAT_FIFO_MERGE,
         FEAT_OMAP3_DSI_FIFO_BUG,
+        FEAT_DPI_USES_VDDS_DSI,
+};
+
+static const enum dss_feat_id am35xx_dss_feat_list[] = {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_LINEBUFFERSPLIT,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+        FEAT_DSI_PLL_FREQSEL,
+        FEAT_DSI_REVERSE_TXCLKESC,
+        FEAT_VENC_REQUIRES_TV_DAC_CLK,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FIXED_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_OMAP3_DSI_FIFO_BUG,
 };
 
 static const enum dss_feat_id omap3630_dss_feat_list[] = {
@@ -447,6 +602,28 @@ static const enum dss_feat_id omap4_dss_feat_list[] = {
         FEAT_BURST_2D,
 };
 
+static const enum dss_feat_id omap5_dss_feat_list[] = {
+        FEAT_MGR_LCD2,
+        FEAT_CORE_CLK_DIV,
+        FEAT_LCD_CLK_SRC,
+        FEAT_DSI_DCS_CMD_CONFIG_VC,
+        FEAT_DSI_VC_OCP_WIDTH,
+        FEAT_DSI_GNQ,
+        FEAT_HDMI_CTS_SWMODE,
+        FEAT_HDMI_AUDIO_USE_MCLK,
+        FEAT_HANDLE_UV_SEPARATE,
+        FEAT_ATTR2,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FREE_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_BURST_2D,
+        FEAT_DSI_PLL_SELFREQDCO,
+        FEAT_DSI_PLL_REFSEL,
+        FEAT_DSI_PHY_DCC,
+};
+
 /* OMAP2 DSS Features */
 static const struct omap_dss_features omap2_dss_features = {
         .reg_fields = omap2_dss_reg_fields,
@@ -458,6 +635,7 @@ static const struct omap_dss_features omap2_dss_features = {
         .num_mgrs = 2,
         .num_ovls = 3,
         .supported_displays = omap2_dss_supported_displays,
+        .supported_outputs = omap2_dss_supported_outputs,
         .supported_color_modes = omap2_dss_supported_color_modes,
         .overlay_caps = omap2_dss_overlay_caps,
         .clksrc_names = omap2_dss_clk_source_names,
@@ -478,6 +656,31 @@ static const struct omap_dss_features omap3430_dss_features = {
         .num_mgrs = 2,
         .num_ovls = 3,
         .supported_displays = omap3430_dss_supported_displays,
+        .supported_outputs = omap3430_dss_supported_outputs,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .overlay_caps = omap3430_dss_overlay_caps,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+        .buffer_size_unit = 1,
+        .burst_size_unit = 8,
+};
+
+/*
+ * AM35xx DSS Features. This is basically OMAP3 DSS Features without the
+ * vdds_dsi regulator.
+ */
+static const struct omap_dss_features am35xx_dss_features = {
+        .reg_fields = omap3_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+        .features = am35xx_dss_feat_list,
+        .num_features = ARRAY_SIZE(am35xx_dss_feat_list),
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3430_dss_supported_displays,
+        .supported_outputs = omap3430_dss_supported_outputs,
         .supported_color_modes = omap3_dss_supported_color_modes,
         .overlay_caps = omap3430_dss_overlay_caps,
         .clksrc_names = omap3_dss_clk_source_names,
@@ -497,6 +700,7 @@ static const struct omap_dss_features omap3630_dss_features = {
         .num_mgrs = 2,
         .num_ovls = 3,
         .supported_displays = omap3630_dss_supported_displays,
+        .supported_outputs = omap3630_dss_supported_outputs,
         .supported_color_modes = omap3_dss_supported_color_modes,
         .overlay_caps = omap3630_dss_overlay_caps,
         .clksrc_names = omap3_dss_clk_source_names,
@@ -517,7 +721,9 @@ static const struct omap_dss_features omap4430_es1_0_dss_features  = {
 
         .num_mgrs = 3,
         .num_ovls = 4,
+        .num_wbs = 1,
         .supported_displays = omap4_dss_supported_displays,
+        .supported_outputs = omap4_dss_supported_outputs,
         .supported_color_modes = omap4_dss_supported_color_modes,
         .overlay_caps = omap4_dss_overlay_caps,
         .clksrc_names = omap4_dss_clk_source_names,
@@ -537,7 +743,9 @@ static const struct omap_dss_features omap4430_es2_0_1_2_dss_features = {
 
         .num_mgrs = 3,
         .num_ovls = 4,
+        .num_wbs = 1,
         .supported_displays = omap4_dss_supported_displays,
+        .supported_outputs = omap4_dss_supported_outputs,
         .supported_color_modes = omap4_dss_supported_color_modes,
         .overlay_caps = omap4_dss_overlay_caps,
         .clksrc_names = omap4_dss_clk_source_names,
@@ -557,7 +765,9 @@ static const struct omap_dss_features omap4_dss_features = {
 
         .num_mgrs = 3,
         .num_ovls = 4,
+        .num_wbs = 1,
         .supported_displays = omap4_dss_supported_displays,
+        .supported_outputs = omap4_dss_supported_outputs,
         .supported_color_modes = omap4_dss_supported_color_modes,
         .overlay_caps = omap4_dss_overlay_caps,
         .clksrc_names = omap4_dss_clk_source_names,
@@ -567,6 +777,27 @@ static const struct omap_dss_features omap4_dss_features = {
         .burst_size_unit = 16,
 };
 
+/* OMAP5 DSS Features */
+static const struct omap_dss_features omap5_dss_features = {
+        .reg_fields = omap5_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap5_dss_reg_fields),
+
+        .features = omap5_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap5_dss_feat_list),
+
+        .num_mgrs = 3,
+        .num_ovls = 4,
+        .supported_displays = omap5_dss_supported_displays,
+        .supported_outputs = omap5_dss_supported_outputs,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .overlay_caps = omap4_dss_overlay_caps,
+        .clksrc_names = omap5_dss_clk_source_names,
+        .dss_params = omap5_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+        .buffer_size_unit = 16,
+        .burst_size_unit = 16,
+};
+
 #if defined(CONFIG_OMAP4_DSS_HDMI)
 /* HDMI OMAP4 Functions*/
 static const struct ti_hdmi_ip_ops omap4_hdmi_functions = {
@@ -612,6 +843,11 @@ int dss_feat_get_num_ovls(void)
         return omap_current_dss_features->num_ovls;
 }
 
+int dss_feat_get_num_wbs(void)
+{
+        return omap_current_dss_features->num_wbs;
+}
+
 unsigned long dss_feat_get_param_min(enum dss_range_param param)
 {
         return omap_current_dss_features->dss_params[param].min;
@@ -627,6 +863,11 @@ enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel
         return omap_current_dss_features->supported_displays[channel];
 }
 
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel)
+{
+        return omap_current_dss_features->supported_outputs[channel];
+}
+
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
 {
         return omap_current_dss_features->supported_color_modes[plane];
@@ -694,8 +935,13 @@ void dss_features_init(void)
                 omap_current_dss_features = &omap2_dss_features;
         else if (cpu_is_omap3630())
                 omap_current_dss_features = &omap3630_dss_features;
-        else if (cpu_is_omap34xx())
-                omap_current_dss_features = &omap3430_dss_features;
+        else if (cpu_is_omap34xx()) {
+                if (soc_is_am35xx()) {
+                        omap_current_dss_features = &am35xx_dss_features;
+                } else {
+                        omap_current_dss_features = &omap3430_dss_features;
+                }
+        }
         else if (omap_rev() == OMAP4430_REV_ES1_0)
                 omap_current_dss_features = &omap4430_es1_0_dss_features;
         else if (omap_rev() == OMAP4430_REV_ES2_0 ||
@@ -704,6 +950,8 @@ void dss_features_init(void)
                 omap_current_dss_features = &omap4430_es2_0_1_2_dss_features;
         else if (cpu_is_omap44xx())
                 omap_current_dss_features = &omap4_dss_features;
+        else if (soc_is_omap54xx())
+                omap_current_dss_features = &omap5_dss_features;
         else
                 DSSWARN("Unsupported OMAP version");
 }
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
index 996ffcbfed58..9218113b5e88 100644
--- a/drivers/video/omap2/dss/dss_features.h
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -50,6 +50,7 @@ enum dss_feat_id {
         FEAT_DSI_VC_OCP_WIDTH,
         FEAT_DSI_REVERSE_TXCLKESC,
         FEAT_DSI_GNQ,
+        FEAT_DPI_USES_VDDS_DSI,
         FEAT_HDMI_CTS_SWMODE,
         FEAT_HDMI_AUDIO_USE_MCLK,
         FEAT_HANDLE_UV_SEPARATE,
@@ -64,6 +65,9 @@ enum dss_feat_id {
         /* An unknown HW bug causing the normal FIFO thresholds not to work */
         FEAT_OMAP3_DSI_FIFO_BUG,
         FEAT_BURST_2D,
+        FEAT_DSI_PLL_SELFREQDCO,
+        FEAT_DSI_PLL_REFSEL,
+        FEAT_DSI_PHY_DCC,
 };
 
 /* DSS register field id */
@@ -91,6 +95,7 @@ enum dss_range_param {
         FEAT_PARAM_DSIPLL_REGM_DSI,
         FEAT_PARAM_DSIPLL_FINT,
         FEAT_PARAM_DSIPLL_LPDIV,
+        FEAT_PARAM_DSI_FCK,
         FEAT_PARAM_DOWNSCALE,
         FEAT_PARAM_LINEWIDTH,
         FEAT_PARAM_MGR_WIDTH,
@@ -100,9 +105,11 @@ enum dss_range_param {
 /* DSS Feature Functions */
 int dss_feat_get_num_mgrs(void);
 int dss_feat_get_num_ovls(void);
+int dss_feat_get_num_wbs(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_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane);
 enum omap_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane);
 bool dss_feat_color_mode_supported(enum omap_plane plane,
diff --git a/drivers/video/omap2/dss/hdmi.c b/drivers/video/omap2/dss/hdmi.c
index 060216fdc578..a48a7dd75b33 100644
--- a/drivers/video/omap2/dss/hdmi.c
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -32,6 +32,8 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
 #include <video/omapdss.h>
 
 #include "ti_hdmi.h"
@@ -61,6 +63,13 @@ static struct {
         struct hdmi_ip_data ip_data;
 
         struct clk *sys_clk;
+        struct regulator *vdda_hdmi_dac_reg;
+
+        int ct_cp_hpd_gpio;
+        int ls_oe_gpio;
+        int hpd_gpio;
+
+        struct omap_dss_output output;
 } hdmi;
 
 /*
@@ -314,12 +323,47 @@ static void hdmi_runtime_put(void)
 
 static int __init hdmi_init_display(struct omap_dss_device *dssdev)
 {
+        int r;
+
+        struct gpio gpios[] = {
+                { hdmi.ct_cp_hpd_gpio, GPIOF_OUT_INIT_LOW, "hdmi_ct_cp_hpd" },
+                { hdmi.ls_oe_gpio, GPIOF_OUT_INIT_LOW, "hdmi_ls_oe" },
+                { hdmi.hpd_gpio, GPIOF_DIR_IN, "hdmi_hpd" },
+        };
+
         DSSDBG("init_display\n");
 
         dss_init_hdmi_ip_ops(&hdmi.ip_data);
+
+        if (hdmi.vdda_hdmi_dac_reg == NULL) {
+                struct regulator *reg;
+
+                reg = devm_regulator_get(&hdmi.pdev->dev, "vdda_hdmi_dac");
+
+                if (IS_ERR(reg)) {
+                        DSSERR("can't get VDDA_HDMI_DAC regulator\n");
+                        return PTR_ERR(reg);
+                }
+
+                hdmi.vdda_hdmi_dac_reg = reg;
+        }
+
+        r = gpio_request_array(gpios, ARRAY_SIZE(gpios));
+        if (r)
+                return r;
+
         return 0;
 }
 
+static void __exit hdmi_uninit_display(struct omap_dss_device *dssdev)
+{
+        DSSDBG("uninit_display\n");
+
+        gpio_free(hdmi.ct_cp_hpd_gpio);
+        gpio_free(hdmi.ls_oe_gpio);
+        gpio_free(hdmi.hpd_gpio);
+}
+
 static const struct hdmi_config *hdmi_find_timing(
                                         const struct hdmi_config *timings_arr,
                                         int len)
@@ -459,32 +503,30 @@ static void hdmi_compute_pll(struct omap_dss_device *dssdev, int phy,
 static int hdmi_power_on(struct omap_dss_device *dssdev)
 {
         int r;
-        const struct hdmi_config *timing;
         struct omap_video_timings *p;
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         unsigned long phy;
 
+        gpio_set_value(hdmi.ct_cp_hpd_gpio, 1);
+        gpio_set_value(hdmi.ls_oe_gpio, 1);
+
+        /* wait 300us after CT_CP_HPD for the 5V power output to reach 90% */
+        udelay(300);
+
+        r = regulator_enable(hdmi.vdda_hdmi_dac_reg);
+        if (r)
+                goto err_vdac_enable;
+
         r = hdmi_runtime_get();
         if (r)
-                return r;
+                goto err_runtime_get;
 
-        dss_mgr_disable(dssdev->manager);
+        dss_mgr_disable(mgr);
 
-        p = &dssdev->panel.timings;
+        p = &hdmi.ip_data.cfg.timings;
 
-        DSSDBG("hdmi_power_on x_res= %d y_res = %d\n",
-                dssdev->panel.timings.x_res,
-                dssdev->panel.timings.y_res);
+        DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
 
-        timing = hdmi_get_timings();
-        if (timing == NULL) {
-                /* HDMI code 4 corresponds to 640 * 480 VGA */
-                hdmi.ip_data.cfg.cm.code = 4;
-                /* DVI mode 1 corresponds to HDMI 0 to DVI */
-                hdmi.ip_data.cfg.cm.mode = HDMI_DVI;
-                hdmi.ip_data.cfg = vesa_timings[0];
-        } else {
-                hdmi.ip_data.cfg = *timing;
-        }
         phy = p->pixel_clock;
 
         hdmi_compute_pll(dssdev, phy, &hdmi.ip_data.pll_data);
@@ -495,13 +537,13 @@ static int hdmi_power_on(struct omap_dss_device *dssdev)
         r = hdmi.ip_data.ops->pll_enable(&hdmi.ip_data);
         if (r) {
                 DSSDBG("Failed to lock PLL\n");
-                goto err;
+                goto err_pll_enable;
         }
 
         r = hdmi.ip_data.ops->phy_enable(&hdmi.ip_data);
         if (r) {
                 DSSDBG("Failed to start PHY\n");
-                goto err;
+                goto err_phy_enable;
         }
 
         hdmi.ip_data.ops->video_configure(&hdmi.ip_data);
@@ -521,13 +563,13 @@ static int hdmi_power_on(struct omap_dss_device *dssdev)
         dispc_enable_gamma_table(0);
 
         /* tv size */
-        dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
+        dss_mgr_set_timings(mgr, p);
 
         r = hdmi.ip_data.ops->video_enable(&hdmi.ip_data);
         if (r)
                 goto err_vid_enable;
 
-        r = dss_mgr_enable(dssdev->manager);
+        r = dss_mgr_enable(mgr);
         if (r)
                 goto err_mgr_enable;
 
@@ -537,20 +579,33 @@ err_mgr_enable:
         hdmi.ip_data.ops->video_disable(&hdmi.ip_data);
 err_vid_enable:
         hdmi.ip_data.ops->phy_disable(&hdmi.ip_data);
+err_phy_enable:
         hdmi.ip_data.ops->pll_disable(&hdmi.ip_data);
-err:
+err_pll_enable:
         hdmi_runtime_put();
+err_runtime_get:
+        regulator_disable(hdmi.vdda_hdmi_dac_reg);
+err_vdac_enable:
+        gpio_set_value(hdmi.ct_cp_hpd_gpio, 0);
+        gpio_set_value(hdmi.ls_oe_gpio, 0);
         return -EIO;
 }
 
 static void hdmi_power_off(struct omap_dss_device *dssdev)
 {
-        dss_mgr_disable(dssdev->manager);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+        dss_mgr_disable(mgr);
 
         hdmi.ip_data.ops->video_disable(&hdmi.ip_data);
         hdmi.ip_data.ops->phy_disable(&hdmi.ip_data);
         hdmi.ip_data.ops->pll_disable(&hdmi.ip_data);
         hdmi_runtime_put();
+
+        regulator_disable(hdmi.vdda_hdmi_dac_reg);
+
+        gpio_set_value(hdmi.ct_cp_hpd_gpio, 0);
+        gpio_set_value(hdmi.ls_oe_gpio, 0);
 }
 
 int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
@@ -567,25 +622,22 @@ int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
 
 }
 
-void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev)
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
 {
         struct hdmi_cm cm;
+        const struct hdmi_config *t;
 
-        cm = hdmi_get_code(&dssdev->panel.timings);
-        hdmi.ip_data.cfg.cm.code = cm.code;
-        hdmi.ip_data.cfg.cm.mode = cm.mode;
+        mutex_lock(&hdmi.lock);
 
-        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-                int r;
+        cm = hdmi_get_code(timings);
+        hdmi.ip_data.cfg.cm = cm;
 
-                hdmi_power_off(dssdev);
+        t = hdmi_get_timings();
+        if (t != NULL)
+                hdmi.ip_data.cfg = *t;
 
-                r = hdmi_power_on(dssdev);
-                if (r)
-                        DSSERR("failed to power on device\n");
-        } else {
-                dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
-        }
+        mutex_unlock(&hdmi.lock);
 }
 
 static void hdmi_dump_regs(struct seq_file *s)
@@ -640,20 +692,20 @@ bool omapdss_hdmi_detect(void)
 
 int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
 {
-        struct omap_dss_hdmi_data *priv = dssdev->data;
+        struct omap_dss_output *out = dssdev->output;
         int r = 0;
 
         DSSDBG("ENTER hdmi_display_enable\n");
 
         mutex_lock(&hdmi.lock);
 
-        if (dssdev->manager == NULL) {
-                DSSERR("failed to enable display: no manager\n");
+        if (out == NULL || out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
                 r = -ENODEV;
                 goto err0;
         }
 
-        hdmi.ip_data.hpd_gpio = priv->hpd_gpio;
+        hdmi.ip_data.hpd_gpio = hdmi.hpd_gpio;
 
         r = omap_dss_start_device(dssdev);
         if (r) {
@@ -661,26 +713,15 @@ int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
                 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;
+                goto err1;
         }
 
         mutex_unlock(&hdmi.lock);
         return 0;
 
-err2:
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
 err1:
         omap_dss_stop_device(dssdev);
 err0:
@@ -696,9 +737,6 @@ void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev)
 
         hdmi_power_off(dssdev);
 
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
         omap_dss_stop_device(dssdev);
 
         mutex_unlock(&hdmi.lock);
@@ -869,10 +907,14 @@ int hdmi_audio_config(struct omap_dss_audio *audio)
 
 #endif
 
-static void __init hdmi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init hdmi_find_dssdev(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int r, i;
+        const char *def_disp_name = dss_get_default_display_name();
+        struct omap_dss_device *def_dssdev;
+        int i;
+
+        def_dssdev = NULL;
 
         for (i = 0; i < pdata->num_devices; ++i) {
                 struct omap_dss_device *dssdev = pdata->devices[i];
@@ -880,17 +922,76 @@ static void __init hdmi_probe_pdata(struct platform_device *pdev)
                 if (dssdev->type != OMAP_DISPLAY_TYPE_HDMI)
                         continue;
 
-                r = hdmi_init_display(dssdev);
-                if (r) {
-                        DSSERR("device %s init failed: %d\n", dssdev->name, r);
-                        continue;
+                if (def_dssdev == NULL)
+                        def_dssdev = dssdev;
+
+                if (def_disp_name != NULL &&
+                                strcmp(dssdev->name, def_disp_name) == 0) {
+                        def_dssdev = dssdev;
+                        break;
                 }
+        }
 
-                r = omap_dss_register_device(dssdev, &pdev->dev, i);
-                if (r)
-                        DSSERR("device %s register failed: %d\n",
-                                        dssdev->name, r);
+        return def_dssdev;
+}
+
+static void __init hdmi_probe_pdata(struct platform_device *pdev)
+{
+        struct omap_dss_device *plat_dssdev;
+        struct omap_dss_device *dssdev;
+        struct omap_dss_hdmi_data *priv;
+        int r;
+
+        plat_dssdev = hdmi_find_dssdev(pdev);
+
+        if (!plat_dssdev)
+                return;
+
+        dssdev = dss_alloc_and_init_device(&pdev->dev);
+        if (!dssdev)
+                return;
+
+        dss_copy_device_pdata(dssdev, plat_dssdev);
+
+        priv = dssdev->data;
+
+        hdmi.ct_cp_hpd_gpio = priv->ct_cp_hpd_gpio;
+        hdmi.ls_oe_gpio = priv->ls_oe_gpio;
+        hdmi.hpd_gpio = priv->hpd_gpio;
+
+        dssdev->channel = OMAP_DSS_CHANNEL_DIGIT;
+
+        r = hdmi_init_display(dssdev);
+        if (r) {
+                DSSERR("device %s init failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
         }
+
+        r = dss_add_device(dssdev);
+        if (r) {
+                DSSERR("device %s register failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
+        }
+}
+
+static void __init hdmi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &hdmi.output;
+
+        out->pdev = pdev;
+        out->id = OMAP_DSS_OUTPUT_HDMI;
+        out->type = OMAP_DISPLAY_TYPE_HDMI;
+
+        dss_register_output(out);
+}
+
+static void __exit hdmi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &hdmi.output;
+
+        dss_unregister_output(out);
 }
 
 /* HDMI HW IP initialisation */
@@ -929,23 +1030,37 @@ static int __init omapdss_hdmihw_probe(struct platform_device *pdev)
         hdmi.ip_data.core_av_offset = HDMI_CORE_AV;
         hdmi.ip_data.pll_offset = HDMI_PLLCTRL;
         hdmi.ip_data.phy_offset = HDMI_PHY;
+
         mutex_init(&hdmi.ip_data.lock);
 
         hdmi_panel_init();
 
         dss_debugfs_create_file("hdmi", hdmi_dump_regs);
 
+        hdmi_init_output(pdev);
+
         hdmi_probe_pdata(pdev);
 
         return 0;
 }
 
+static int __exit hdmi_remove_child(struct device *dev, void *data)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        hdmi_uninit_display(dssdev);
+        return 0;
+}
+
 static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
 {
-        omap_dss_unregister_child_devices(&pdev->dev);
+        device_for_each_child(&pdev->dev, NULL, hdmi_remove_child);
+
+        dss_unregister_child_devices(&pdev->dev);
 
         hdmi_panel_exit();
 
+        hdmi_uninit_output(pdev);
+
         pm_runtime_disable(&pdev->dev);
 
         hdmi_put_clocks();
diff --git a/drivers/video/omap2/dss/hdmi_panel.c b/drivers/video/omap2/dss/hdmi_panel.c
index e10844faadf9..69fb115bab32 100644
--- a/drivers/video/omap2/dss/hdmi_panel.c
+++ b/drivers/video/omap2/dss/hdmi_panel.c
@@ -41,17 +41,34 @@ static struct {
 
 static int hdmi_panel_probe(struct omap_dss_device *dssdev)
 {
+        /* Initialize default timings to VGA in DVI mode */
+        const struct omap_video_timings default_timings = {
+                .x_res          = 640,
+                .y_res          = 480,
+                .pixel_clock    = 25175,
+                .hsw            = 96,
+                .hfp            = 16,
+                .hbp            = 48,
+                .vsw            = 2,
+                .vfp            = 11,
+                .vbp            = 31,
+
+                .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+                .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+
+                .interlace      = false,
+        };
+
         DSSDBG("ENTER hdmi_panel_probe\n");
 
-        dssdev->panel.timings = (struct omap_video_timings)
-                        { 640, 480, 25175, 96, 16, 48, 2, 11, 31,
-                                OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
-                                false,
-                        };
+        dssdev->panel.timings = default_timings;
 
         DSSDBG("hdmi_panel_probe x_res= %d y_res = %d\n",
                 dssdev->panel.timings.x_res,
                 dssdev->panel.timings.y_res);
+
+        omapdss_hdmi_display_set_timing(dssdev, &dssdev->panel.timings);
+
         return 0;
 }
 
@@ -228,6 +245,8 @@ static int hdmi_panel_enable(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        omapdss_hdmi_display_set_timing(dssdev, &dssdev->panel.timings);
+
         r = omapdss_hdmi_display_enable(dssdev);
         if (r) {
                 DSSERR("failed to power on\n");
@@ -336,8 +355,8 @@ static void hdmi_set_timings(struct omap_dss_device *dssdev,
          */
         hdmi_panel_audio_disable(dssdev);
 
+        omapdss_hdmi_display_set_timing(dssdev, timings);
         dssdev->panel.timings = *timings;
-        omapdss_hdmi_display_set_timing(dssdev);
 
         mutex_unlock(&hdmi.lock);
 }
diff --git a/drivers/video/omap2/dss/manager-sysfs.c b/drivers/video/omap2/dss/manager-sysfs.c
new file mode 100644
index 000000000000..9a2fb59b6f89
--- /dev/null
+++ b/drivers/video/omap2/dss/manager-sysfs.c
@@ -0,0 +1,512 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "MANAGER"
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t manager_name_show(struct omap_overlay_manager *mgr, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n", mgr->name);
+}
+
+static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
+{
+        struct omap_dss_device *dssdev = mgr->get_device(mgr);
+
+        return snprintf(buf, PAGE_SIZE, "%s\n", dssdev ?
+                        dssdev->name : "<none>");
+}
+
+static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        int r = 0;
+        size_t len = size;
+        struct omap_dss_device *dssdev = NULL;
+
+        int match(struct omap_dss_device *dssdev, void *data)
+        {
+                const char *str = data;
+                return sysfs_streq(dssdev->name, str);
+        }
+
+        if (buf[size-1] == '\n')
+                --len;
+
+        if (len > 0)
+                dssdev = omap_dss_find_device((void *)buf, match);
+
+        if (len > 0 && dssdev == NULL)
+                return -EINVAL;
+
+        if (dssdev)
+                DSSDBG("display %s found\n", dssdev->name);
+
+        if (mgr->output) {
+                r = mgr->unset_output(mgr);
+                if (r) {
+                        DSSERR("failed to unset current output\n");
+                        goto put_device;
+                }
+        }
+
+        if (dssdev) {
+                struct omap_dss_output *out = dssdev->output;
+
+                /*
+                 * a registered device should have an output connected to it
+                 * already
+                 */
+                if (!out) {
+                        DSSERR("device has no output connected to it\n");
+                        goto put_device;
+                }
+
+                r = mgr->set_output(mgr, out);
+                if (r) {
+                        DSSERR("failed to set manager output\n");
+                        goto put_device;
+                }
+
+                r = mgr->apply(mgr);
+                if (r) {
+                        DSSERR("failed to apply dispc config\n");
+                        goto put_device;
+                }
+        }
+
+put_device:
+        if (dssdev)
+                omap_dss_put_device(dssdev);
+
+        return r ? r : size;
+}
+
+static ssize_t manager_default_color_show(struct omap_overlay_manager *mgr,
+                                          char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%#x\n", info.default_color);
+}
+
+static ssize_t manager_default_color_store(struct omap_overlay_manager *mgr,
+                                           const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        u32 color;
+        int r;
+
+        r = kstrtouint(buf, 0, &color);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.default_color = color;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static const char *trans_key_type_str[] = {
+        "gfx-destination",
+        "video-source",
+};
+
+static ssize_t manager_trans_key_type_show(struct omap_overlay_manager *mgr,
+                                           char *buf)
+{
+        enum omap_dss_trans_key_type key_type;
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        key_type = info.trans_key_type;
+        BUG_ON(key_type >= ARRAY_SIZE(trans_key_type_str));
+
+        return snprintf(buf, PAGE_SIZE, "%s\n", trans_key_type_str[key_type]);
+}
+
+static ssize_t manager_trans_key_type_store(struct omap_overlay_manager *mgr,
+                                            const char *buf, size_t size)
+{
+        enum omap_dss_trans_key_type key_type;
+        struct omap_overlay_manager_info info;
+        int r;
+
+        for (key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
+                        key_type < ARRAY_SIZE(trans_key_type_str); key_type++) {
+                if (sysfs_streq(buf, trans_key_type_str[key_type]))
+                        break;
+        }
+
+        if (key_type == ARRAY_SIZE(trans_key_type_str))
+                return -EINVAL;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.trans_key_type = key_type;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_trans_key_value_show(struct omap_overlay_manager *mgr,
+                                            char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%#x\n", info.trans_key);
+}
+
+static ssize_t manager_trans_key_value_store(struct omap_overlay_manager *mgr,
+                                             const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        u32 key_value;
+        int r;
+
+        r = kstrtouint(buf, 0, &key_value);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.trans_key = key_value;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_trans_key_enabled_show(struct omap_overlay_manager *mgr,
+                                              char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.trans_enabled);
+}
+
+static ssize_t manager_trans_key_enabled_store(struct omap_overlay_manager *mgr,
+                                               const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        bool enable;
+        int r;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.trans_enabled = enable;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_alpha_blending_enabled_show(
+                struct omap_overlay_manager *mgr, char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
+
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                info.partial_alpha_enabled);
+}
+
+static ssize_t manager_alpha_blending_enabled_store(
+                struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        bool enable;
+        int r;
+
+        WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.partial_alpha_enabled = enable;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
+                char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.cpr_enable);
+}
+
+static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        int r;
+        bool enable;
+
+        if (!dss_has_feature(FEAT_CPR))
+                return -ENODEV;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        if (info.cpr_enable == enable)
+                return size;
+
+        info.cpr_enable = enable;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
+                char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE,
+                        "%d %d %d %d %d %d %d %d %d\n",
+                        info.cpr_coefs.rr,
+                        info.cpr_coefs.rg,
+                        info.cpr_coefs.rb,
+                        info.cpr_coefs.gr,
+                        info.cpr_coefs.gg,
+                        info.cpr_coefs.gb,
+                        info.cpr_coefs.br,
+                        info.cpr_coefs.bg,
+                        info.cpr_coefs.bb);
+}
+
+static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        struct omap_dss_cpr_coefs coefs;
+        int r, i;
+        s16 *arr;
+
+        if (!dss_has_feature(FEAT_CPR))
+                return -ENODEV;
+
+        if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
+                                &coefs.rr, &coefs.rg, &coefs.rb,
+                                &coefs.gr, &coefs.gg, &coefs.gb,
+                                &coefs.br, &coefs.bg, &coefs.bb) != 9)
+                return -EINVAL;
+
+        arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
+                coefs.gr, coefs.gg, coefs.gb,
+                coefs.br, coefs.bg, coefs.bb };
+
+        for (i = 0; i < 9; ++i) {
+                if (arr[i] < -512 || arr[i] > 511)
+                        return -EINVAL;
+        }
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.cpr_coefs = coefs;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+struct manager_attribute {
+        struct attribute attr;
+        ssize_t (*show)(struct omap_overlay_manager *, char *);
+        ssize_t (*store)(struct omap_overlay_manager *, const char *, size_t);
+};
+
+#define MANAGER_ATTR(_name, _mode, _show, _store) \
+        struct manager_attribute manager_attr_##_name = \
+        __ATTR(_name, _mode, _show, _store)
+
+static MANAGER_ATTR(name, S_IRUGO, manager_name_show, NULL);
+static MANAGER_ATTR(display, S_IRUGO|S_IWUSR,
+                manager_display_show, manager_display_store);
+static MANAGER_ATTR(default_color, S_IRUGO|S_IWUSR,
+                manager_default_color_show, manager_default_color_store);
+static MANAGER_ATTR(trans_key_type, S_IRUGO|S_IWUSR,
+                manager_trans_key_type_show, manager_trans_key_type_store);
+static MANAGER_ATTR(trans_key_value, S_IRUGO|S_IWUSR,
+                manager_trans_key_value_show, manager_trans_key_value_store);
+static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
+                manager_trans_key_enabled_show,
+                manager_trans_key_enabled_store);
+static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
+                manager_alpha_blending_enabled_show,
+                manager_alpha_blending_enabled_store);
+static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
+                manager_cpr_enable_show,
+                manager_cpr_enable_store);
+static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
+                manager_cpr_coef_show,
+                manager_cpr_coef_store);
+
+
+static struct attribute *manager_sysfs_attrs[] = {
+        &manager_attr_name.attr,
+        &manager_attr_display.attr,
+        &manager_attr_default_color.attr,
+        &manager_attr_trans_key_type.attr,
+        &manager_attr_trans_key_value.attr,
+        &manager_attr_trans_key_enabled.attr,
+        &manager_attr_alpha_blending_enabled.attr,
+        &manager_attr_cpr_enable.attr,
+        &manager_attr_cpr_coef.attr,
+        NULL
+};
+
+static ssize_t manager_attr_show(struct kobject *kobj, struct attribute *attr,
+                char *buf)
+{
+        struct omap_overlay_manager *manager;
+        struct manager_attribute *manager_attr;
+
+        manager = container_of(kobj, struct omap_overlay_manager, kobj);
+        manager_attr = container_of(attr, struct manager_attribute, attr);
+
+        if (!manager_attr->show)
+                return -ENOENT;
+
+        return manager_attr->show(manager, buf);
+}
+
+static ssize_t manager_attr_store(struct kobject *kobj, struct attribute *attr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager *manager;
+        struct manager_attribute *manager_attr;
+
+        manager = container_of(kobj, struct omap_overlay_manager, kobj);
+        manager_attr = container_of(attr, struct manager_attribute, attr);
+
+        if (!manager_attr->store)
+                return -ENOENT;
+
+        return manager_attr->store(manager, buf, size);
+}
+
+static const struct sysfs_ops manager_sysfs_ops = {
+        .show = manager_attr_show,
+        .store = manager_attr_store,
+};
+
+static struct kobj_type manager_ktype = {
+        .sysfs_ops = &manager_sysfs_ops,
+        .default_attrs = manager_sysfs_attrs,
+};
+
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+                struct platform_device *pdev)
+{
+        return kobject_init_and_add(&mgr->kobj, &manager_ktype,
+                        &pdev->dev.kobj, "manager%d", mgr->id);
+}
+
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr)
+{
+        kobject_del(&mgr->kobj);
+        kobject_put(&mgr->kobj);
+}
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 53710fadc82d..c54d2f620ce3 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -36,463 +36,15 @@
 static int num_managers;
 static struct omap_overlay_manager *managers;
 
-static ssize_t manager_name_show(struct omap_overlay_manager *mgr, char *buf)
+static inline struct omap_dss_device *dss_mgr_get_device(struct omap_overlay_manager *mgr)
 {
-        return snprintf(buf, PAGE_SIZE, "%s\n", mgr->name);
+        return mgr->output ? mgr->output->device : NULL;
 }
 
-static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
-{
-        return snprintf(buf, PAGE_SIZE, "%s\n",
-                        mgr->device ? mgr->device->name : "<none>");
-}
-
-static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
-                const char *buf, size_t size)
-{
-        int r = 0;
-        size_t len = size;
-        struct omap_dss_device *dssdev = NULL;
-
-        int match(struct omap_dss_device *dssdev, void *data)
-        {
-                const char *str = data;
-                return sysfs_streq(dssdev->name, str);
-        }
-
-        if (buf[size-1] == '\n')
-                --len;
-
-        if (len > 0)
-                dssdev = omap_dss_find_device((void *)buf, match);
-
-        if (len > 0 && dssdev == NULL)
-                return -EINVAL;
-
-        if (dssdev)
-                DSSDBG("display %s found\n", dssdev->name);
-
-        if (mgr->device) {
-                r = mgr->unset_device(mgr);
-                if (r) {
-                        DSSERR("failed to unset display\n");
-                        goto put_device;
-                }
-        }
-
-        if (dssdev) {
-                r = mgr->set_device(mgr, dssdev);
-                if (r) {
-                        DSSERR("failed to set manager\n");
-                        goto put_device;
-                }
-
-                r = mgr->apply(mgr);
-                if (r) {
-                        DSSERR("failed to apply dispc config\n");
-                        goto put_device;
-                }
-        }
-
-put_device:
-        if (dssdev)
-                omap_dss_put_device(dssdev);
-
-        return r ? r : size;
-}
-
-static ssize_t manager_default_color_show(struct omap_overlay_manager *mgr,
-                                          char *buf)
-{
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%#x\n", info.default_color);
-}
-
-static ssize_t manager_default_color_store(struct omap_overlay_manager *mgr,
-                                           const char *buf, size_t size)
-{
-        struct omap_overlay_manager_info info;
-        u32 color;
-        int r;
-
-        r = kstrtouint(buf, 0, &color);
-        if (r)
-                return r;
-
-        mgr->get_manager_info(mgr, &info);
-
-        info.default_color = color;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-static const char *trans_key_type_str[] = {
-        "gfx-destination",
-        "video-source",
-};
-
-static ssize_t manager_trans_key_type_show(struct omap_overlay_manager *mgr,
-                                           char *buf)
-{
-        enum omap_dss_trans_key_type key_type;
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        key_type = info.trans_key_type;
-        BUG_ON(key_type >= ARRAY_SIZE(trans_key_type_str));
-
-        return snprintf(buf, PAGE_SIZE, "%s\n", trans_key_type_str[key_type]);
-}
-
-static ssize_t manager_trans_key_type_store(struct omap_overlay_manager *mgr,
-                                            const char *buf, size_t size)
-{
-        enum omap_dss_trans_key_type key_type;
-        struct omap_overlay_manager_info info;
-        int r;
-
-        for (key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
-                        key_type < ARRAY_SIZE(trans_key_type_str); key_type++) {
-                if (sysfs_streq(buf, trans_key_type_str[key_type]))
-                        break;
-        }
-
-        if (key_type == ARRAY_SIZE(trans_key_type_str))
-                return -EINVAL;
-
-        mgr->get_manager_info(mgr, &info);
-
-        info.trans_key_type = key_type;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-static ssize_t manager_trans_key_value_show(struct omap_overlay_manager *mgr,
-                                            char *buf)
-{
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%#x\n", info.trans_key);
-}
-
-static ssize_t manager_trans_key_value_store(struct omap_overlay_manager *mgr,
-                                             const char *buf, size_t size)
-{
-        struct omap_overlay_manager_info info;
-        u32 key_value;
-        int r;
-
-        r = kstrtouint(buf, 0, &key_value);
-        if (r)
-                return r;
-
-        mgr->get_manager_info(mgr, &info);
-
-        info.trans_key = key_value;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-static ssize_t manager_trans_key_enabled_show(struct omap_overlay_manager *mgr,
-                                              char *buf)
-{
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d\n", info.trans_enabled);
-}
-
-static ssize_t manager_trans_key_enabled_store(struct omap_overlay_manager *mgr,
-                                               const char *buf, size_t size)
-{
-        struct omap_overlay_manager_info info;
-        bool enable;
-        int r;
-
-        r = strtobool(buf, &enable);
-        if (r)
-                return r;
-
-        mgr->get_manager_info(mgr, &info);
-
-        info.trans_enabled = enable;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-static ssize_t manager_alpha_blending_enabled_show(
-                struct omap_overlay_manager *mgr, char *buf)
-{
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
-
-        return snprintf(buf, PAGE_SIZE, "%d\n",
-                info.partial_alpha_enabled);
-}
-
-static ssize_t manager_alpha_blending_enabled_store(
-                struct omap_overlay_manager *mgr,
-                const char *buf, size_t size)
-{
-        struct omap_overlay_manager_info info;
-        bool enable;
-        int r;
-
-        WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
-
-        r = strtobool(buf, &enable);
-        if (r)
-                return r;
-
-        mgr->get_manager_info(mgr, &info);
-
-        info.partial_alpha_enabled = enable;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
-                char *buf)
-{
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d\n", info.cpr_enable);
-}
-
-static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
-                const char *buf, size_t size)
-{
-        struct omap_overlay_manager_info info;
-        int r;
-        bool enable;
-
-        if (!dss_has_feature(FEAT_CPR))
-                return -ENODEV;
-
-        r = strtobool(buf, &enable);
-        if (r)
-                return r;
-
-        mgr->get_manager_info(mgr, &info);
-
-        if (info.cpr_enable == enable)
-                return size;
-
-        info.cpr_enable = enable;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
-                char *buf)
-{
-        struct omap_overlay_manager_info info;
-
-        mgr->get_manager_info(mgr, &info);
-
-        return snprintf(buf, PAGE_SIZE,
-                        "%d %d %d %d %d %d %d %d %d\n",
-                        info.cpr_coefs.rr,
-                        info.cpr_coefs.rg,
-                        info.cpr_coefs.rb,
-                        info.cpr_coefs.gr,
-                        info.cpr_coefs.gg,
-                        info.cpr_coefs.gb,
-                        info.cpr_coefs.br,
-                        info.cpr_coefs.bg,
-                        info.cpr_coefs.bb);
-}
-
-static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
-                const char *buf, size_t size)
-{
-        struct omap_overlay_manager_info info;
-        struct omap_dss_cpr_coefs coefs;
-        int r, i;
-        s16 *arr;
-
-        if (!dss_has_feature(FEAT_CPR))
-                return -ENODEV;
-
-        if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
-                                &coefs.rr, &coefs.rg, &coefs.rb,
-                                &coefs.gr, &coefs.gg, &coefs.gb,
-                                &coefs.br, &coefs.bg, &coefs.bb) != 9)
-                return -EINVAL;
-
-        arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
-                coefs.gr, coefs.gg, coefs.gb,
-                coefs.br, coefs.bg, coefs.bb };
-
-        for (i = 0; i < 9; ++i) {
-                if (arr[i] < -512 || arr[i] > 511)
-                        return -EINVAL;
-        }
-
-        mgr->get_manager_info(mgr, &info);
-
-        info.cpr_coefs = coefs;
-
-        r = mgr->set_manager_info(mgr, &info);
-        if (r)
-                return r;
-
-        r = mgr->apply(mgr);
-        if (r)
-                return r;
-
-        return size;
-}
-
-struct manager_attribute {
-        struct attribute attr;
-        ssize_t (*show)(struct omap_overlay_manager *, char *);
-        ssize_t (*store)(struct omap_overlay_manager *, const char *, size_t);
-};
-
-#define MANAGER_ATTR(_name, _mode, _show, _store) \
-        struct manager_attribute manager_attr_##_name = \
-        __ATTR(_name, _mode, _show, _store)
-
-static MANAGER_ATTR(name, S_IRUGO, manager_name_show, NULL);
-static MANAGER_ATTR(display, S_IRUGO|S_IWUSR,
-                manager_display_show, manager_display_store);
-static MANAGER_ATTR(default_color, S_IRUGO|S_IWUSR,
-                manager_default_color_show, manager_default_color_store);
-static MANAGER_ATTR(trans_key_type, S_IRUGO|S_IWUSR,
-                manager_trans_key_type_show, manager_trans_key_type_store);
-static MANAGER_ATTR(trans_key_value, S_IRUGO|S_IWUSR,
-                manager_trans_key_value_show, manager_trans_key_value_store);
-static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
-                manager_trans_key_enabled_show,
-                manager_trans_key_enabled_store);
-static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
-                manager_alpha_blending_enabled_show,
-                manager_alpha_blending_enabled_store);
-static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
-                manager_cpr_enable_show,
-                manager_cpr_enable_store);
-static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
-                manager_cpr_coef_show,
-                manager_cpr_coef_store);
-
-
-static struct attribute *manager_sysfs_attrs[] = {
-        &manager_attr_name.attr,
-        &manager_attr_display.attr,
-        &manager_attr_default_color.attr,
-        &manager_attr_trans_key_type.attr,
-        &manager_attr_trans_key_value.attr,
-        &manager_attr_trans_key_enabled.attr,
-        &manager_attr_alpha_blending_enabled.attr,
-        &manager_attr_cpr_enable.attr,
-        &manager_attr_cpr_coef.attr,
-        NULL
-};
-
-static ssize_t manager_attr_show(struct kobject *kobj, struct attribute *attr,
-                char *buf)
-{
-        struct omap_overlay_manager *manager;
-        struct manager_attribute *manager_attr;
-
-        manager = container_of(kobj, struct omap_overlay_manager, kobj);
-        manager_attr = container_of(attr, struct manager_attribute, attr);
-
-        if (!manager_attr->show)
-                return -ENOENT;
-
-        return manager_attr->show(manager, buf);
-}
-
-static ssize_t manager_attr_store(struct kobject *kobj, struct attribute *attr,
-                const char *buf, size_t size)
-{
-        struct omap_overlay_manager *manager;
-        struct manager_attribute *manager_attr;
-
-        manager = container_of(kobj, struct omap_overlay_manager, kobj);
-        manager_attr = container_of(attr, struct manager_attribute, attr);
-
-        if (!manager_attr->store)
-                return -ENOENT;
-
-        return manager_attr->store(manager, buf, size);
-}
-
-static const struct sysfs_ops manager_sysfs_ops = {
-        .show = manager_attr_show,
-        .store = manager_attr_store,
-};
-
-static struct kobj_type manager_ktype = {
-        .sysfs_ops = &manager_sysfs_ops,
-        .default_attrs = manager_sysfs_attrs,
-};
-
 static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
 {
         unsigned long timeout = msecs_to_jiffies(500);
+        struct omap_dss_device *dssdev = mgr->get_device(mgr);
         u32 irq;
         int r;
 
@@ -500,9 +52,9 @@ static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
         if (r)
                 return r;
 
-        if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC)
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC)
                 irq = DISPC_IRQ_EVSYNC_ODD;
-        else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI)
+        else if (dssdev->type == OMAP_DISPLAY_TYPE_HDMI)
                 irq = DISPC_IRQ_EVSYNC_EVEN;
         else
                 irq = dispc_mgr_get_vsync_irq(mgr->id);
@@ -547,23 +99,24 @@ int dss_init_overlay_managers(struct platform_device *pdev)
                         break;
                 }
 
-                mgr->set_device = &dss_mgr_set_device;
-                mgr->unset_device = &dss_mgr_unset_device;
+                mgr->set_output = &dss_mgr_set_output;
+                mgr->unset_output = &dss_mgr_unset_output;
                 mgr->apply = &omap_dss_mgr_apply;
                 mgr->set_manager_info = &dss_mgr_set_info;
                 mgr->get_manager_info = &dss_mgr_get_info;
                 mgr->wait_for_go = &dss_mgr_wait_for_go;
                 mgr->wait_for_vsync = &dss_mgr_wait_for_vsync;
+                mgr->get_device = &dss_mgr_get_device;
 
                 mgr->caps = 0;
                 mgr->supported_displays =
                         dss_feat_get_supported_displays(mgr->id);
+                mgr->supported_outputs =
+                        dss_feat_get_supported_outputs(mgr->id);
 
                 INIT_LIST_HEAD(&mgr->overlays);
 
-                r = kobject_init_and_add(&mgr->kobj, &manager_ktype,
-                                &pdev->dev.kobj, "manager%d", i);
-
+                r = dss_manager_kobj_init(mgr, pdev);
                 if (r)
                         DSSERR("failed to create sysfs file\n");
         }
@@ -577,9 +130,7 @@ void dss_uninit_overlay_managers(struct platform_device *pdev)
 
         for (i = 0; i < num_managers; ++i) {
                 struct omap_overlay_manager *mgr = &managers[i];
-
-                kobject_del(&mgr->kobj);
-                kobject_put(&mgr->kobj);
+                dss_manager_kobj_uninit(mgr);
         }
 
         kfree(managers);
diff --git a/drivers/video/omap2/dss/output.c b/drivers/video/omap2/dss/output.c
new file mode 100644
index 000000000000..813f26682b7a
--- /dev/null
+++ b/drivers/video/omap2/dss/output.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2012 Texas Instruments Ltd
+ * Author: Archit Taneja <archit@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/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static LIST_HEAD(output_list);
+static DEFINE_MUTEX(output_lock);
+
+int omapdss_output_set_device(struct omap_dss_output *out,
+                struct omap_dss_device *dssdev)
+{
+        int r;
+
+        mutex_lock(&output_lock);
+
+        if (out->device) {
+                DSSERR("output already has device %s connected to it\n",
+                        out->device->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        if (out->type != dssdev->type) {
+                DSSERR("output type and display type don't match\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        out->device = dssdev;
+        dssdev->output = out;
+
+        mutex_unlock(&output_lock);
+
+        return 0;
+err:
+        mutex_unlock(&output_lock);
+
+        return r;
+}
+EXPORT_SYMBOL(omapdss_output_set_device);
+
+int omapdss_output_unset_device(struct omap_dss_output *out)
+{
+        int r;
+
+        mutex_lock(&output_lock);
+
+        if (!out->device) {
+                DSSERR("output doesn't have a device connected to it\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        if (out->device->state != OMAP_DSS_DISPLAY_DISABLED) {
+                DSSERR("device %s is not disabled, cannot unset device\n",
+                                out->device->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        out->device->output = NULL;
+        out->device = NULL;
+
+        mutex_unlock(&output_lock);
+
+        return 0;
+err:
+        mutex_unlock(&output_lock);
+
+        return r;
+}
+EXPORT_SYMBOL(omapdss_output_unset_device);
+
+void dss_register_output(struct omap_dss_output *out)
+{
+        list_add_tail(&out->list, &output_list);
+}
+
+void dss_unregister_output(struct omap_dss_output *out)
+{
+        list_del(&out->list);
+}
+
+struct omap_dss_output *omap_dss_get_output(enum omap_dss_output_id id)
+{
+        struct omap_dss_output *out;
+
+        list_for_each_entry(out, &output_list, list) {
+                if (out->id == id)
+                        return out;
+        }
+
+        return NULL;
+}
+
+struct omap_dss_output *omapdss_get_output_from_dssdev(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_output *out = NULL;
+        enum omap_dss_output_id id;
+
+        switch (dssdev->type) {
+        case OMAP_DISPLAY_TYPE_DPI:
+                out = omap_dss_get_output(OMAP_DSS_OUTPUT_DPI);
+                break;
+        case OMAP_DISPLAY_TYPE_DBI:
+                out = omap_dss_get_output(OMAP_DSS_OUTPUT_DBI);
+                break;
+        case OMAP_DISPLAY_TYPE_SDI:
+                out = omap_dss_get_output(OMAP_DSS_OUTPUT_SDI);
+                break;
+        case OMAP_DISPLAY_TYPE_VENC:
+                out = omap_dss_get_output(OMAP_DSS_OUTPUT_VENC);
+                break;
+        case OMAP_DISPLAY_TYPE_HDMI:
+                out = omap_dss_get_output(OMAP_DSS_OUTPUT_HDMI);
+                break;
+        case OMAP_DISPLAY_TYPE_DSI:
+                id = dssdev->phy.dsi.module == 0 ? OMAP_DSS_OUTPUT_DSI1 :
+                                        OMAP_DSS_OUTPUT_DSI2;
+                out = omap_dss_get_output(id);
+                break;
+        default:
+                break;
+        }
+
+        return out;
+}
diff --git a/drivers/video/omap2/dss/overlay-sysfs.c b/drivers/video/omap2/dss/overlay-sysfs.c
new file mode 100644
index 000000000000..4cc5ddebfb34
--- /dev/null
+++ b/drivers/video/omap2/dss/overlay-sysfs.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "OVERLAY"
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/platform_device.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t overlay_name_show(struct omap_overlay *ovl, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n", ovl->name);
+}
+
+static ssize_t overlay_manager_show(struct omap_overlay *ovl, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n",
+                        ovl->manager ? ovl->manager->name : "<none>");
+}
+
+static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
+                size_t size)
+{
+        int i, r;
+        struct omap_overlay_manager *mgr = NULL;
+        struct omap_overlay_manager *old_mgr;
+        int len = size;
+
+        if (buf[size-1] == '\n')
+                --len;
+
+        if (len > 0) {
+                for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+                        mgr = omap_dss_get_overlay_manager(i);
+
+                        if (sysfs_streq(buf, mgr->name))
+                                break;
+
+                        mgr = NULL;
+                }
+        }
+
+        if (len > 0 && mgr == NULL)
+                return -EINVAL;
+
+        if (mgr)
+                DSSDBG("manager %s found\n", mgr->name);
+
+        if (mgr == ovl->manager)
+                return size;
+
+        old_mgr = ovl->manager;
+
+        r = dispc_runtime_get();
+        if (r)
+                return r;
+
+        /* detach old manager */
+        if (old_mgr) {
+                r = ovl->unset_manager(ovl);
+                if (r) {
+                        DSSERR("detach failed\n");
+                        goto err;
+                }
+
+                r = old_mgr->apply(old_mgr);
+                if (r)
+                        goto err;
+        }
+
+        if (mgr) {
+                r = ovl->set_manager(ovl, mgr);
+                if (r) {
+                        DSSERR("Failed to attach overlay\n");
+                        goto err;
+                }
+
+                r = mgr->apply(mgr);
+                if (r)
+                        goto err;
+        }
+
+        dispc_runtime_put();
+
+        return size;
+
+err:
+        dispc_runtime_put();
+        return r;
+}
+
+static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+                        info.width, info.height);
+}
+
+static ssize_t overlay_screen_width_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.screen_width);
+}
+
+static ssize_t overlay_position_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+                        info.pos_x, info.pos_y);
+}
+
+static ssize_t overlay_position_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        char *last;
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.pos_x = simple_strtoul(buf, &last, 10);
+        ++last;
+        if (last - buf >= size)
+                return -EINVAL;
+
+        info.pos_y = simple_strtoul(last, &last, 10);
+
+        r = ovl->set_overlay_info(ovl, &info);
+        if (r)
+                return r;
+
+        if (ovl->manager) {
+                r = ovl->manager->apply(ovl->manager);
+                if (r)
+                        return r;
+        }
+
+        return size;
+}
+
+static ssize_t overlay_output_size_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+                        info.out_width, info.out_height);
+}
+
+static ssize_t overlay_output_size_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        char *last;
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.out_width = simple_strtoul(buf, &last, 10);
+        ++last;
+        if (last - buf >= size)
+                return -EINVAL;
+
+        info.out_height = simple_strtoul(last, &last, 10);
+
+        r = ovl->set_overlay_info(ovl, &info);
+        if (r)
+                return r;
+
+        if (ovl->manager) {
+                r = ovl->manager->apply(ovl->manager);
+                if (r)
+                        return r;
+        }
+
+        return size;
+}
+
+static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%d\n", ovl->is_enabled(ovl));
+}
+
+static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
+                size_t size)
+{
+        int r;
+        bool enable;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        if (enable)
+                r = ovl->enable(ovl);
+        else
+                r = ovl->disable(ovl);
+
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t overlay_global_alpha_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        info.global_alpha);
+}
+
+static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        u8 alpha;
+        struct omap_overlay_info info;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+                return -ENODEV;
+
+        r = kstrtou8(buf, 0, &alpha);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.global_alpha = alpha;
+
+        r = ovl->set_overlay_info(ovl, &info);
+        if (r)
+                return r;
+
+        if (ovl->manager) {
+                r = ovl->manager->apply(ovl->manager);
+                if (r)
+                        return r;
+        }
+
+        return size;
+}
+
+static ssize_t overlay_pre_mult_alpha_show(struct omap_overlay *ovl,
+                char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        info.pre_mult_alpha);
+}
+
+static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        u8 alpha;
+        struct omap_overlay_info info;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+                return -ENODEV;
+
+        r = kstrtou8(buf, 0, &alpha);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.pre_mult_alpha = alpha;
+
+        r = ovl->set_overlay_info(ovl, &info);
+        if (r)
+                return r;
+
+        if (ovl->manager) {
+                r = ovl->manager->apply(ovl->manager);
+                if (r)
+                        return r;
+        }
+
+        return size;
+}
+
+static ssize_t overlay_zorder_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.zorder);
+}
+
+static ssize_t overlay_zorder_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        u8 zorder;
+        struct omap_overlay_info info;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+                return -ENODEV;
+
+        r = kstrtou8(buf, 0, &zorder);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.zorder = zorder;
+
+        r = ovl->set_overlay_info(ovl, &info);
+        if (r)
+                return r;
+
+        if (ovl->manager) {
+                r = ovl->manager->apply(ovl->manager);
+                if (r)
+                        return r;
+        }
+
+        return size;
+}
+
+struct overlay_attribute {
+        struct attribute attr;
+        ssize_t (*show)(struct omap_overlay *, char *);
+        ssize_t (*store)(struct omap_overlay *, const char *, size_t);
+};
+
+#define OVERLAY_ATTR(_name, _mode, _show, _store) \
+        struct overlay_attribute overlay_attr_##_name = \
+        __ATTR(_name, _mode, _show, _store)
+
+static OVERLAY_ATTR(name, S_IRUGO, overlay_name_show, NULL);
+static OVERLAY_ATTR(manager, S_IRUGO|S_IWUSR,
+                overlay_manager_show, overlay_manager_store);
+static OVERLAY_ATTR(input_size, S_IRUGO, overlay_input_size_show, NULL);
+static OVERLAY_ATTR(screen_width, S_IRUGO, overlay_screen_width_show, NULL);
+static OVERLAY_ATTR(position, S_IRUGO|S_IWUSR,
+                overlay_position_show, overlay_position_store);
+static OVERLAY_ATTR(output_size, S_IRUGO|S_IWUSR,
+                overlay_output_size_show, overlay_output_size_store);
+static OVERLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
+                overlay_enabled_show, overlay_enabled_store);
+static OVERLAY_ATTR(global_alpha, S_IRUGO|S_IWUSR,
+                overlay_global_alpha_show, overlay_global_alpha_store);
+static OVERLAY_ATTR(pre_mult_alpha, S_IRUGO|S_IWUSR,
+                overlay_pre_mult_alpha_show,
+                overlay_pre_mult_alpha_store);
+static OVERLAY_ATTR(zorder, S_IRUGO|S_IWUSR,
+                overlay_zorder_show, overlay_zorder_store);
+
+static struct attribute *overlay_sysfs_attrs[] = {
+        &overlay_attr_name.attr,
+        &overlay_attr_manager.attr,
+        &overlay_attr_input_size.attr,
+        &overlay_attr_screen_width.attr,
+        &overlay_attr_position.attr,
+        &overlay_attr_output_size.attr,
+        &overlay_attr_enabled.attr,
+        &overlay_attr_global_alpha.attr,
+        &overlay_attr_pre_mult_alpha.attr,
+        &overlay_attr_zorder.attr,
+        NULL
+};
+
+static ssize_t overlay_attr_show(struct kobject *kobj, struct attribute *attr,
+                char *buf)
+{
+        struct omap_overlay *overlay;
+        struct overlay_attribute *overlay_attr;
+
+        overlay = container_of(kobj, struct omap_overlay, kobj);
+        overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+        if (!overlay_attr->show)
+                return -ENOENT;
+
+        return overlay_attr->show(overlay, buf);
+}
+
+static ssize_t overlay_attr_store(struct kobject *kobj, struct attribute *attr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay *overlay;
+        struct overlay_attribute *overlay_attr;
+
+        overlay = container_of(kobj, struct omap_overlay, kobj);
+        overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+        if (!overlay_attr->store)
+                return -ENOENT;
+
+        return overlay_attr->store(overlay, buf, size);
+}
+
+static const struct sysfs_ops overlay_sysfs_ops = {
+        .show = overlay_attr_show,
+        .store = overlay_attr_store,
+};
+
+static struct kobj_type overlay_ktype = {
+        .sysfs_ops = &overlay_sysfs_ops,
+        .default_attrs = overlay_sysfs_attrs,
+};
+
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+                struct platform_device *pdev)
+{
+        return kobject_init_and_add(&ovl->kobj, &overlay_ktype,
+                        &pdev->dev.kobj, "overlay%d", ovl->id);
+}
+
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl)
+{
+        kobject_del(&ovl->kobj);
+        kobject_put(&ovl->kobj);
+}
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 952c6fad9a81..45f4994bc6b0 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -26,13 +26,11 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/sysfs.h>
-#include <linux/kobject.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
 
 #include <video/omapdss.h>
-#include <plat/cpu.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -40,417 +38,13 @@
 static int num_overlays;
 static struct omap_overlay *overlays;
 
-static ssize_t overlay_name_show(struct omap_overlay *ovl, char *buf)
+static inline struct omap_dss_device *dss_ovl_get_device(struct omap_overlay *ovl)
 {
-        return snprintf(buf, PAGE_SIZE, "%s\n", ovl->name);
+        return ovl->manager ?
+                (ovl->manager->output ? ovl->manager->output->device : NULL) :
+                NULL;
 }
 
-static ssize_t overlay_manager_show(struct omap_overlay *ovl, char *buf)
-{
-        return snprintf(buf, PAGE_SIZE, "%s\n",
-                        ovl->manager ? ovl->manager->name : "<none>");
-}
-
-static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
-                size_t size)
-{
-        int i, r;
-        struct omap_overlay_manager *mgr = NULL;
-        struct omap_overlay_manager *old_mgr;
-        int len = size;
-
-        if (buf[size-1] == '\n')
-                --len;
-
-        if (len > 0) {
-                for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
-                        mgr = omap_dss_get_overlay_manager(i);
-
-                        if (sysfs_streq(buf, mgr->name))
-                                break;
-
-                        mgr = NULL;
-                }
-        }
-
-        if (len > 0 && mgr == NULL)
-                return -EINVAL;
-
-        if (mgr)
-                DSSDBG("manager %s found\n", mgr->name);
-
-        if (mgr == ovl->manager)
-                return size;
-
-        old_mgr = ovl->manager;
-
-        r = dispc_runtime_get();
-        if (r)
-                return r;
-
-        /* detach old manager */
-        if (old_mgr) {
-                r = ovl->unset_manager(ovl);
-                if (r) {
-                        DSSERR("detach failed\n");
-                        goto err;
-                }
-
-                r = old_mgr->apply(old_mgr);
-                if (r)
-                        goto err;
-        }
-
-        if (mgr) {
-                r = ovl->set_manager(ovl, mgr);
-                if (r) {
-                        DSSERR("Failed to attach overlay\n");
-                        goto err;
-                }
-
-                r = mgr->apply(mgr);
-                if (r)
-                        goto err;
-        }
-
-        dispc_runtime_put();
-
-        return size;
-
-err:
-        dispc_runtime_put();
-        return r;
-}
-
-static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
-                        info.width, info.height);
-}
-
-static ssize_t overlay_screen_width_show(struct omap_overlay *ovl, char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d\n", info.screen_width);
-}
-
-static ssize_t overlay_position_show(struct omap_overlay *ovl, char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
-                        info.pos_x, info.pos_y);
-}
-
-static ssize_t overlay_position_store(struct omap_overlay *ovl,
-                const char *buf, size_t size)
-{
-        int r;
-        char *last;
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        info.pos_x = simple_strtoul(buf, &last, 10);
-        ++last;
-        if (last - buf >= size)
-                return -EINVAL;
-
-        info.pos_y = simple_strtoul(last, &last, 10);
-
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                return r;
-
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
-                if (r)
-                        return r;
-        }
-
-        return size;
-}
-
-static ssize_t overlay_output_size_show(struct omap_overlay *ovl, char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
-                        info.out_width, info.out_height);
-}
-
-static ssize_t overlay_output_size_store(struct omap_overlay *ovl,
-                const char *buf, size_t size)
-{
-        int r;
-        char *last;
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        info.out_width = simple_strtoul(buf, &last, 10);
-        ++last;
-        if (last - buf >= size)
-                return -EINVAL;
-
-        info.out_height = simple_strtoul(last, &last, 10);
-
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                return r;
-
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
-                if (r)
-                        return r;
-        }
-
-        return size;
-}
-
-static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
-{
-        return snprintf(buf, PAGE_SIZE, "%d\n", ovl->is_enabled(ovl));
-}
-
-static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
-                size_t size)
-{
-        int r;
-        bool enable;
-
-        r = strtobool(buf, &enable);
-        if (r)
-                return r;
-
-        if (enable)
-                r = ovl->enable(ovl);
-        else
-                r = ovl->disable(ovl);
-
-        if (r)
-                return r;
-
-        return size;
-}
-
-static ssize_t overlay_global_alpha_show(struct omap_overlay *ovl, char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d\n",
-                        info.global_alpha);
-}
-
-static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
-                const char *buf, size_t size)
-{
-        int r;
-        u8 alpha;
-        struct omap_overlay_info info;
-
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
-                return -ENODEV;
-
-        r = kstrtou8(buf, 0, &alpha);
-        if (r)
-                return r;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        info.global_alpha = alpha;
-
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                return r;
-
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
-                if (r)
-                        return r;
-        }
-
-        return size;
-}
-
-static ssize_t overlay_pre_mult_alpha_show(struct omap_overlay *ovl,
-                char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d\n",
-                        info.pre_mult_alpha);
-}
-
-static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
-                const char *buf, size_t size)
-{
-        int r;
-        u8 alpha;
-        struct omap_overlay_info info;
-
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
-                return -ENODEV;
-
-        r = kstrtou8(buf, 0, &alpha);
-        if (r)
-                return r;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        info.pre_mult_alpha = alpha;
-
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                return r;
-
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
-                if (r)
-                        return r;
-        }
-
-        return size;
-}
-
-static ssize_t overlay_zorder_show(struct omap_overlay *ovl, char *buf)
-{
-        struct omap_overlay_info info;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        return snprintf(buf, PAGE_SIZE, "%d\n", info.zorder);
-}
-
-static ssize_t overlay_zorder_store(struct omap_overlay *ovl,
-                const char *buf, size_t size)
-{
-        int r;
-        u8 zorder;
-        struct omap_overlay_info info;
-
-        if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
-                return -ENODEV;
-
-        r = kstrtou8(buf, 0, &zorder);
-        if (r)
-                return r;
-
-        ovl->get_overlay_info(ovl, &info);
-
-        info.zorder = zorder;
-
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                return r;
-
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
-                if (r)
-                        return r;
-        }
-
-        return size;
-}
-
-struct overlay_attribute {
-        struct attribute attr;
-        ssize_t (*show)(struct omap_overlay *, char *);
-        ssize_t (*store)(struct omap_overlay *, const char *, size_t);
-};
-
-#define OVERLAY_ATTR(_name, _mode, _show, _store) \
-        struct overlay_attribute overlay_attr_##_name = \
-        __ATTR(_name, _mode, _show, _store)
-
-static OVERLAY_ATTR(name, S_IRUGO, overlay_name_show, NULL);
-static OVERLAY_ATTR(manager, S_IRUGO|S_IWUSR,
-                overlay_manager_show, overlay_manager_store);
-static OVERLAY_ATTR(input_size, S_IRUGO, overlay_input_size_show, NULL);
-static OVERLAY_ATTR(screen_width, S_IRUGO, overlay_screen_width_show, NULL);
-static OVERLAY_ATTR(position, S_IRUGO|S_IWUSR,
-                overlay_position_show, overlay_position_store);
-static OVERLAY_ATTR(output_size, S_IRUGO|S_IWUSR,
-                overlay_output_size_show, overlay_output_size_store);
-static OVERLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
-                overlay_enabled_show, overlay_enabled_store);
-static OVERLAY_ATTR(global_alpha, S_IRUGO|S_IWUSR,
-                overlay_global_alpha_show, overlay_global_alpha_store);
-static OVERLAY_ATTR(pre_mult_alpha, S_IRUGO|S_IWUSR,
-                overlay_pre_mult_alpha_show,
-                overlay_pre_mult_alpha_store);
-static OVERLAY_ATTR(zorder, S_IRUGO|S_IWUSR,
-                overlay_zorder_show, overlay_zorder_store);
-
-static struct attribute *overlay_sysfs_attrs[] = {
-        &overlay_attr_name.attr,
-        &overlay_attr_manager.attr,
-        &overlay_attr_input_size.attr,
-        &overlay_attr_screen_width.attr,
-        &overlay_attr_position.attr,
-        &overlay_attr_output_size.attr,
-        &overlay_attr_enabled.attr,
-        &overlay_attr_global_alpha.attr,
-        &overlay_attr_pre_mult_alpha.attr,
-        &overlay_attr_zorder.attr,
-        NULL
-};
-
-static ssize_t overlay_attr_show(struct kobject *kobj, struct attribute *attr,
-                char *buf)
-{
-        struct omap_overlay *overlay;
-        struct overlay_attribute *overlay_attr;
-
-        overlay = container_of(kobj, struct omap_overlay, kobj);
-        overlay_attr = container_of(attr, struct overlay_attribute, attr);
-
-        if (!overlay_attr->show)
-                return -ENOENT;
-
-        return overlay_attr->show(overlay, buf);
-}
-
-static ssize_t overlay_attr_store(struct kobject *kobj, struct attribute *attr,
-                const char *buf, size_t size)
-{
-        struct omap_overlay *overlay;
-        struct overlay_attribute *overlay_attr;
-
-        overlay = container_of(kobj, struct omap_overlay, kobj);
-        overlay_attr = container_of(attr, struct overlay_attribute, attr);
-
-        if (!overlay_attr->store)
-                return -ENOENT;
-
-        return overlay_attr->store(overlay, buf, size);
-}
-
-static const struct sysfs_ops overlay_sysfs_ops = {
-        .show = overlay_attr_show,
-        .store = overlay_attr_store,
-};
-
-static struct kobj_type overlay_ktype = {
-        .sysfs_ops = &overlay_sysfs_ops,
-        .default_attrs = overlay_sysfs_attrs,
-};
-
 int omap_dss_get_num_overlays(void)
 {
         return num_overlays;
@@ -507,97 +101,25 @@ void dss_init_overlays(struct platform_device *pdev)
                 ovl->set_overlay_info = &dss_ovl_set_info;
                 ovl->get_overlay_info = &dss_ovl_get_info;
                 ovl->wait_for_go = &dss_mgr_wait_for_go_ovl;
+                ovl->get_device = &dss_ovl_get_device;
 
                 ovl->caps = dss_feat_get_overlay_caps(ovl->id);
                 ovl->supported_modes =
                         dss_feat_get_supported_color_modes(ovl->id);
 
-                r = kobject_init_and_add(&ovl->kobj, &overlay_ktype,
-                                &pdev->dev.kobj, "overlay%d", i);
-
+                r = dss_overlay_kobj_init(ovl, pdev);
                 if (r)
                         DSSERR("failed to create sysfs file\n");
         }
 }
 
-/* connect overlays to the new device, if not already connected. if force
- * selected, connect always. */
-void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
-{
-        int i;
-        struct omap_overlay_manager *lcd_mgr;
-        struct omap_overlay_manager *tv_mgr;
-        struct omap_overlay_manager *lcd2_mgr = NULL;
-        struct omap_overlay_manager *lcd3_mgr = NULL;
-        struct omap_overlay_manager *mgr = NULL;
-
-        lcd_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD);
-        tv_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_DIGIT);
-        if (dss_has_feature(FEAT_MGR_LCD3))
-                lcd3_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD3);
-        if (dss_has_feature(FEAT_MGR_LCD2))
-                lcd2_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD2);
-
-        if (dssdev->channel == OMAP_DSS_CHANNEL_LCD3) {
-                if (!lcd3_mgr->device || force) {
-                        if (lcd3_mgr->device)
-                                lcd3_mgr->unset_device(lcd3_mgr);
-                        lcd3_mgr->set_device(lcd3_mgr, dssdev);
-                        mgr = lcd3_mgr;
-                }
-        } else if (dssdev->channel == OMAP_DSS_CHANNEL_LCD2) {
-                if (!lcd2_mgr->device || force) {
-                        if (lcd2_mgr->device)
-                                lcd2_mgr->unset_device(lcd2_mgr);
-                        lcd2_mgr->set_device(lcd2_mgr, dssdev);
-                        mgr = lcd2_mgr;
-                }
-        } 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);
-                        lcd_mgr->set_device(lcd_mgr, dssdev);
-                        mgr = lcd_mgr;
-                }
-        }
-
-        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);
-                        tv_mgr->set_device(tv_mgr, dssdev);
-                        mgr = tv_mgr;
-                }
-        }
-
-        if (mgr) {
-                dispc_runtime_get();
-
-                for (i = 0; i < dss_feat_get_num_ovls(); i++) {
-                        struct omap_overlay *ovl;
-                        ovl = omap_dss_get_overlay(i);
-                        if (!ovl->manager || force) {
-                                if (ovl->manager)
-                                        ovl->unset_manager(ovl);
-                                ovl->set_manager(ovl, mgr);
-                        }
-                }
-
-                dispc_runtime_put();
-        }
-}
-
 void dss_uninit_overlays(struct platform_device *pdev)
 {
         int i;
 
         for (i = 0; i < num_overlays; ++i) {
                 struct omap_overlay *ovl = &overlays[i];
-
-                kobject_del(&ovl->kobj);
-                kobject_put(&ovl->kobj);
+                dss_overlay_kobj_uninit(ovl);
         }
 
         kfree(overlays);
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index 7c087424b634..7282e5af3e1a 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -111,6 +111,13 @@ static struct {
         struct omap_dss_device *dssdev[2];
 
         struct semaphore bus_lock;
+
+        struct omap_video_timings timings;
+        int pixel_size;
+        int data_lines;
+        struct rfbi_timings intf_timings;
+
+        struct omap_dss_output output;
 } rfbi;
 
 static inline void rfbi_write_reg(const struct rfbi_reg idx, u32 val)
@@ -300,30 +307,23 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
 }
 EXPORT_SYMBOL(omap_rfbi_write_pixels);
 
-static int rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
-                u16 height, void (*callback)(void *data), void *data)
+static int rfbi_transfer_area(struct omap_dss_device *dssdev,
+                void (*callback)(void *data), void *data)
 {
         u32 l;
         int r;
-        struct omap_video_timings timings = {
-                .hsw            = 1,
-                .hfp            = 1,
-                .hbp            = 1,
-                .vsw            = 1,
-                .vfp            = 0,
-                .vbp            = 0,
-                .x_res          = width,
-                .y_res          = height,
-        };
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+        u16 width = rfbi.timings.x_res;
+        u16 height = rfbi.timings.y_res;
 
         /*BUG_ON(callback == 0);*/
         BUG_ON(rfbi.framedone_callback != NULL);
 
         DSSDBG("rfbi_transfer_area %dx%d\n", width, height);
 
-        dss_mgr_set_timings(dssdev->manager, &timings);
+        dss_mgr_set_timings(mgr, &rfbi.timings);
 
-        r = dss_mgr_enable(dssdev->manager);
+        r = dss_mgr_enable(mgr);
         if (r)
                 return r;
 
@@ -770,62 +770,45 @@ static int rfbi_configure(int rfbi_module, int bpp, int lines)
         return 0;
 }
 
-int omap_rfbi_configure(struct omap_dss_device *dssdev, int pixel_size,
-                int data_lines)
+int omap_rfbi_configure(struct omap_dss_device *dssdev)
 {
-        return rfbi_configure(dssdev->phy.rfbi.channel, pixel_size, data_lines);
+        return rfbi_configure(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+                        rfbi.data_lines);
 }
 EXPORT_SYMBOL(omap_rfbi_configure);
 
-int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
-                u16 *x, u16 *y, u16 *w, u16 *h)
+int omap_rfbi_update(struct omap_dss_device *dssdev, void (*callback)(void *),
+                void *data)
 {
-        u16 dw, dh;
-        struct omap_video_timings timings = {
-                .hsw            = 1,
-                .hfp            = 1,
-                .hbp            = 1,
-                .vsw            = 1,
-                .vfp            = 0,
-                .vbp            = 0,
-                .x_res          = *w,
-                .y_res          = *h,
-        };
-
-        dssdev->driver->get_resolution(dssdev, &dw, &dh);
-
-        if  (*x > dw || *y > dh)
-                return -EINVAL;
-
-        if (*x + *w > dw)
-                return -EINVAL;
-
-        if (*y + *h > dh)
-                return -EINVAL;
-
-        if (*w == 1)
-                return -EINVAL;
-
-        if (*w == 0 || *h == 0)
-                return -EINVAL;
-
-        dss_mgr_set_timings(dssdev->manager, &timings);
+        return rfbi_transfer_area(dssdev, callback, data);
+}
+EXPORT_SYMBOL(omap_rfbi_update);
 
-        return 0;
+void omapdss_rfbi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+{
+        rfbi.timings.x_res = w;
+        rfbi.timings.y_res = h;
 }
-EXPORT_SYMBOL(omap_rfbi_prepare_update);
+EXPORT_SYMBOL(omapdss_rfbi_set_size);
 
-int omap_rfbi_update(struct omap_dss_device *dssdev,
-                u16 x, u16 y, u16 w, u16 h,
-                void (*callback)(void *), void *data)
+void omapdss_rfbi_set_pixel_size(struct omap_dss_device *dssdev, int pixel_size)
 {
-        int r;
+        rfbi.pixel_size = pixel_size;
+}
+EXPORT_SYMBOL(omapdss_rfbi_set_pixel_size);
 
-        r = rfbi_transfer_area(dssdev, w, h, callback, data);
+void omapdss_rfbi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+        rfbi.data_lines = data_lines;
+}
+EXPORT_SYMBOL(omapdss_rfbi_set_data_lines);
 
-        return r;
+void omapdss_rfbi_set_interface_timings(struct omap_dss_device *dssdev,
+                struct rfbi_timings *timings)
+{
+        rfbi.intf_timings = *timings;
 }
-EXPORT_SYMBOL(omap_rfbi_update);
+EXPORT_SYMBOL(omapdss_rfbi_set_interface_timings);
 
 static void rfbi_dump_regs(struct seq_file *s)
 {
@@ -869,6 +852,7 @@ static void rfbi_dump_regs(struct seq_file *s)
 
 static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         struct dss_lcd_mgr_config mgr_config;
 
         mgr_config.io_pad_mode = DSS_IO_PAD_MODE_RFBI;
@@ -877,18 +861,40 @@ static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
         /* Do we need fifohandcheck for RFBI? */
         mgr_config.fifohandcheck = false;
 
-        mgr_config.video_port_width = dssdev->ctrl.pixel_size;
+        mgr_config.video_port_width = rfbi.pixel_size;
         mgr_config.lcden_sig_polarity = 0;
 
-        dss_mgr_set_lcd_config(dssdev->manager, &mgr_config);
+        dss_mgr_set_lcd_config(mgr, &mgr_config);
+
+        /*
+         * Set rfbi.timings with default values, the x_res and y_res fields
+         * are expected to be already configured by the panel driver via
+         * omapdss_rfbi_set_size()
+         */
+        rfbi.timings.hsw = 1;
+        rfbi.timings.hfp = 1;
+        rfbi.timings.hbp = 1;
+        rfbi.timings.vsw = 1;
+        rfbi.timings.vfp = 0;
+        rfbi.timings.vbp = 0;
+
+        rfbi.timings.interlace = false;
+        rfbi.timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        rfbi.timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        rfbi.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        rfbi.timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        rfbi.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+
+        dss_mgr_set_timings(mgr, &rfbi.timings);
 }
 
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
+        struct omap_dss_output *out = dssdev->output;
         int r;
 
-        if (dssdev->manager == NULL) {
-                DSSERR("failed to enable display: no manager\n");
+        if (out == NULL || out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
                 return -ENODEV;
         }
 
@@ -911,13 +917,10 @@ int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 
         rfbi_config_lcd_manager(dssdev);
 
-        rfbi_configure(dssdev->phy.rfbi.channel,
-                               dssdev->ctrl.pixel_size,
-                               dssdev->phy.rfbi.data_lines);
-
-        rfbi_set_timings(dssdev->phy.rfbi.channel,
-                         &dssdev->ctrl.rfbi_timings);
+        rfbi_configure(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+                        rfbi.data_lines);
 
+        rfbi_set_timings(dssdev->phy.rfbi.channel, &rfbi.intf_timings);
 
         return 0;
 err1:
@@ -941,14 +944,17 @@ EXPORT_SYMBOL(omapdss_rfbi_display_disable);
 static int __init rfbi_init_display(struct omap_dss_device *dssdev)
 {
         rfbi.dssdev[dssdev->phy.rfbi.channel] = dssdev;
-        dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
         return 0;
 }
 
-static void __init rfbi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init rfbi_find_dssdev(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int i, r;
+        const char *def_disp_name = dss_get_default_display_name();
+        struct omap_dss_device *def_dssdev;
+        int i;
+
+        def_dssdev = NULL;
 
         for (i = 0; i < pdata->num_devices; ++i) {
                 struct omap_dss_device *dssdev = pdata->devices[i];
@@ -956,17 +962,67 @@ static void __init rfbi_probe_pdata(struct platform_device *pdev)
                 if (dssdev->type != OMAP_DISPLAY_TYPE_DBI)
                         continue;
 
-                r = rfbi_init_display(dssdev);
-                if (r) {
-                        DSSERR("device %s init failed: %d\n", dssdev->name, r);
-                        continue;
+                if (def_dssdev == NULL)
+                        def_dssdev = dssdev;
+
+                if (def_disp_name != NULL &&
+                                strcmp(dssdev->name, def_disp_name) == 0) {
+                        def_dssdev = dssdev;
+                        break;
                 }
+        }
+
+        return def_dssdev;
+}
+
+static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
+{
+        struct omap_dss_device *plat_dssdev;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        plat_dssdev = rfbi_find_dssdev(rfbidev);
+
+        if (!plat_dssdev)
+                return;
+
+        dssdev = dss_alloc_and_init_device(&rfbidev->dev);
+        if (!dssdev)
+                return;
+
+        dss_copy_device_pdata(dssdev, plat_dssdev);
 
-                r = omap_dss_register_device(dssdev, &pdev->dev, i);
-                if (r)
-                        DSSERR("device %s register failed: %d\n",
-                                dssdev->name, r);
+        r = rfbi_init_display(dssdev);
+        if (r) {
+                DSSERR("device %s init failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
         }
+
+        r = dss_add_device(dssdev);
+        if (r) {
+                DSSERR("device %s register failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
+        }
+}
+
+static void __init rfbi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &rfbi.output;
+
+        out->pdev = pdev;
+        out->id = OMAP_DSS_OUTPUT_DBI;
+        out->type = OMAP_DISPLAY_TYPE_DBI;
+
+        dss_register_output(out);
+}
+
+static void __exit rfbi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &rfbi.output;
+
+        dss_unregister_output(out);
 }
 
 /* RFBI HW IP initialisation */
@@ -1020,6 +1076,8 @@ static int __init omap_rfbihw_probe(struct platform_device *pdev)
 
         dss_debugfs_create_file("rfbi", rfbi_dump_regs);
 
+        rfbi_init_output(pdev);
+
         rfbi_probe_pdata(pdev);
 
         return 0;
@@ -1031,8 +1089,12 @@ err_runtime_get:
 
 static int __exit omap_rfbihw_remove(struct platform_device *pdev)
 {
-        omap_dss_unregister_child_devices(&pdev->dev);
+        dss_unregister_child_devices(&pdev->dev);
+
+        rfbi_uninit_output(pdev);
+
         pm_runtime_disable(&pdev->dev);
+
         return 0;
 }
 
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index f43bfe17b3b6..7760851f6e5d 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -25,6 +25,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/export.h>
 #include <linux/platform_device.h>
+#include <linux/string.h>
 
 #include <video/omapdss.h>
 #include "dss.h"
@@ -34,10 +35,16 @@ static struct {
         struct regulator *vdds_sdi_reg;
 
         struct dss_lcd_mgr_config mgr_config;
+        struct omap_video_timings timings;
+        int datapairs;
+
+        struct omap_dss_output output;
 } sdi;
 
 static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
         sdi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
 
         sdi.mgr_config.stallmode = false;
@@ -46,19 +53,20 @@ static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
         sdi.mgr_config.video_port_width = 24;
         sdi.mgr_config.lcden_sig_polarity = 1;
 
-        dss_mgr_set_lcd_config(dssdev->manager, &sdi.mgr_config);
+        dss_mgr_set_lcd_config(mgr, &sdi.mgr_config);
 }
 
 int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 {
-        struct omap_video_timings *t = &dssdev->panel.timings;
+        struct omap_dss_output *out = dssdev->output;
+        struct omap_video_timings *t = &sdi.timings;
         struct dss_clock_info dss_cinfo;
         struct dispc_clock_info dispc_cinfo;
         unsigned long pck;
         int r;
 
-        if (dssdev->manager == NULL) {
-                DSSERR("failed to enable display: no manager\n");
+        if (out == NULL || out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
                 return -ENODEV;
         }
 
@@ -77,8 +85,8 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
                 goto err_get_dispc;
 
         /* 15.5.9.1.2 */
-        dssdev->panel.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
-        dssdev->panel.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        t->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        t->sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
 
         r = dss_calc_clock_div(t->pixel_clock * 1000, &dss_cinfo, &dispc_cinfo);
         if (r)
@@ -97,7 +105,7 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
         }
 
 
-        dss_mgr_set_timings(dssdev->manager, t);
+        dss_mgr_set_timings(out->manager, t);
 
         r = dss_set_clock_div(&dss_cinfo);
         if (r)
@@ -116,16 +124,15 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
          * need to care about the shadow register mechanism for pck-free. The
          * exact reason for this is unknown.
          */
-        dispc_mgr_set_clock_div(dssdev->manager->id,
-                        &sdi.mgr_config.clock_info);
+        dispc_mgr_set_clock_div(out->manager->id, &sdi.mgr_config.clock_info);
 
-        dss_sdi_init(dssdev->phy.sdi.datapairs);
+        dss_sdi_init(sdi.datapairs);
         r = dss_sdi_enable();
         if (r)
                 goto err_sdi_enable;
         mdelay(2);
 
-        r = dss_mgr_enable(dssdev->manager);
+        r = dss_mgr_enable(out->manager);
         if (r)
                 goto err_mgr_enable;
 
@@ -148,7 +155,9 @@ EXPORT_SYMBOL(omapdss_sdi_display_enable);
 
 void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 {
-        dss_mgr_disable(dssdev->manager);
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+        dss_mgr_disable(mgr);
 
         dss_sdi_disable();
 
@@ -160,6 +169,19 @@ void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 }
 EXPORT_SYMBOL(omapdss_sdi_display_disable);
 
+void omapdss_sdi_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        sdi.timings = *timings;
+}
+EXPORT_SYMBOL(omapdss_sdi_set_timings);
+
+void omapdss_sdi_set_datapairs(struct omap_dss_device *dssdev, int datapairs)
+{
+        sdi.datapairs = datapairs;
+}
+EXPORT_SYMBOL(omapdss_sdi_set_datapairs);
+
 static int __init sdi_init_display(struct omap_dss_device *dssdev)
 {
         DSSDBG("SDI init\n");
@@ -180,10 +202,14 @@ static int __init sdi_init_display(struct omap_dss_device *dssdev)
         return 0;
 }
 
-static void __init sdi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init sdi_find_dssdev(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int i, r;
+        const char *def_disp_name = dss_get_default_display_name();
+        struct omap_dss_device *def_dssdev;
+        int i;
+
+        def_dssdev = NULL;
 
         for (i = 0; i < pdata->num_devices; ++i) {
                 struct omap_dss_device *dssdev = pdata->devices[i];
@@ -191,21 +217,73 @@ static void __init sdi_probe_pdata(struct platform_device *pdev)
                 if (dssdev->type != OMAP_DISPLAY_TYPE_SDI)
                         continue;
 
-                r = sdi_init_display(dssdev);
-                if (r) {
-                        DSSERR("device %s init failed: %d\n", dssdev->name, r);
-                        continue;
+                if (def_dssdev == NULL)
+                        def_dssdev = dssdev;
+
+                if (def_disp_name != NULL &&
+                                strcmp(dssdev->name, def_disp_name) == 0) {
+                        def_dssdev = dssdev;
+                        break;
                 }
+        }
+
+        return def_dssdev;
+}
+
+static void __init sdi_probe_pdata(struct platform_device *sdidev)
+{
+        struct omap_dss_device *plat_dssdev;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        plat_dssdev = sdi_find_dssdev(sdidev);
 
-                r = omap_dss_register_device(dssdev, &pdev->dev, i);
-                if (r)
-                        DSSERR("device %s register failed: %d\n",
-                                        dssdev->name, r);
+        if (!plat_dssdev)
+                return;
+
+        dssdev = dss_alloc_and_init_device(&sdidev->dev);
+        if (!dssdev)
+                return;
+
+        dss_copy_device_pdata(dssdev, plat_dssdev);
+
+        r = sdi_init_display(dssdev);
+        if (r) {
+                DSSERR("device %s init failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
         }
+
+        r = dss_add_device(dssdev);
+        if (r) {
+                DSSERR("device %s register failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
+        }
+}
+
+static void __init sdi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &sdi.output;
+
+        out->pdev = pdev;
+        out->id = OMAP_DSS_OUTPUT_SDI;
+        out->type = OMAP_DISPLAY_TYPE_SDI;
+
+        dss_register_output(out);
+}
+
+static void __exit sdi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &sdi.output;
+
+        dss_unregister_output(out);
 }
 
 static int __init omap_sdi_probe(struct platform_device *pdev)
 {
+        sdi_init_output(pdev);
+
         sdi_probe_pdata(pdev);
 
         return 0;
@@ -213,7 +291,9 @@ static int __init omap_sdi_probe(struct platform_device *pdev)
 
 static int __exit omap_sdi_remove(struct platform_device *pdev)
 {
-        omap_dss_unregister_child_devices(&pdev->dev);
+        dss_unregister_child_devices(&pdev->dev);
+
+        sdi_uninit_output(pdev);
 
         return 0;
 }
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index 3a220877461a..56efa3bb465d 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -36,7 +36,6 @@
 #include <linux/pm_runtime.h>
 
 #include <video/omapdss.h>
-#include <plat/cpu.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -300,6 +299,12 @@ static struct {
         struct regulator *vdda_dac_reg;
 
         struct clk      *tv_dac_clk;
+
+        struct omap_video_timings timings;
+        enum omap_dss_venc_type type;
+        bool invert_polarity;
+
+        struct omap_dss_output output;
 } venc;
 
 static inline void venc_write_reg(int idx, u32 val)
@@ -424,65 +429,67 @@ static const struct venc_config *venc_timings_to_config(
 
 static int venc_power_on(struct omap_dss_device *dssdev)
 {
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
         u32 l;
         int r;
 
+        r = venc_runtime_get();
+        if (r)
+                goto err0;
+
         venc_reset();
-        venc_write_config(venc_timings_to_config(&dssdev->panel.timings));
+        venc_write_config(venc_timings_to_config(&venc.timings));
 
-        dss_set_venc_output(dssdev->phy.venc.type);
+        dss_set_venc_output(venc.type);
         dss_set_dac_pwrdn_bgz(1);
 
         l = 0;
 
-        if (dssdev->phy.venc.type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+        if (venc.type == OMAP_DSS_VENC_TYPE_COMPOSITE)
                 l |= 1 << 1;
         else /* S-Video */
                 l |= (1 << 0) | (1 << 2);
 
-        if (dssdev->phy.venc.invert_polarity == false)
+        if (venc.invert_polarity == false)
                 l |= 1 << 3;
 
         venc_write_reg(VENC_OUTPUT_CONTROL, l);
 
-        dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
+        dss_mgr_set_timings(mgr, &venc.timings);
 
         r = regulator_enable(venc.vdda_dac_reg);
         if (r)
-                goto err;
-
-        if (dssdev->platform_enable)
-                dssdev->platform_enable(dssdev);
+                goto err1;
 
-        r = dss_mgr_enable(dssdev->manager);
+        r = dss_mgr_enable(mgr);
         if (r)
-                goto err;
+                goto err2;
 
         return 0;
 
-err:
+err2:
+        regulator_disable(venc.vdda_dac_reg);
+err1:
         venc_write_reg(VENC_OUTPUT_CONTROL, 0);
         dss_set_dac_pwrdn_bgz(0);
 
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
-        regulator_disable(venc.vdda_dac_reg);
-
+        venc_runtime_put();
+err0:
         return r;
 }
 
 static void venc_power_off(struct omap_dss_device *dssdev)
 {
+        struct omap_overlay_manager *mgr = dssdev->output->manager;
+
         venc_write_reg(VENC_OUTPUT_CONTROL, 0);
         dss_set_dac_pwrdn_bgz(0);
 
-        dss_mgr_disable(dssdev->manager);
-
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
+        dss_mgr_disable(mgr);
 
         regulator_disable(venc.vdda_dac_reg);
+
+        venc_runtime_put();
 }
 
 unsigned long venc_get_pixel_clock(void)
@@ -491,171 +498,83 @@ unsigned long venc_get_pixel_clock(void)
         return 13500000;
 }
 
-static ssize_t display_output_type_show(struct device *dev,
-                struct device_attribute *attr, char *buf)
+int omapdss_venc_display_enable(struct omap_dss_device *dssdev)
 {
-        struct omap_dss_device *dssdev = to_dss_device(dev);
-        const char *ret;
-
-        switch (dssdev->phy.venc.type) {
-        case OMAP_DSS_VENC_TYPE_COMPOSITE:
-                ret = "composite";
-                break;
-        case OMAP_DSS_VENC_TYPE_SVIDEO:
-                ret = "svideo";
-                break;
-        default:
-                return -EINVAL;
-        }
-
-        return snprintf(buf, PAGE_SIZE, "%s\n", ret);
-}
+        struct omap_dss_output *out = dssdev->output;
+        int r;
 
-static ssize_t display_output_type_store(struct device *dev,
-                struct device_attribute *attr, const char *buf, size_t size)
-{
-        struct omap_dss_device *dssdev = to_dss_device(dev);
-        enum omap_dss_venc_type new_type;
-
-        if (sysfs_streq("composite", buf))
-                new_type = OMAP_DSS_VENC_TYPE_COMPOSITE;
-        else if (sysfs_streq("svideo", buf))
-                new_type = OMAP_DSS_VENC_TYPE_SVIDEO;
-        else
-                return -EINVAL;
+        DSSDBG("venc_display_enable\n");
 
         mutex_lock(&venc.venc_lock);
 
-        if (dssdev->phy.venc.type != new_type) {
-                dssdev->phy.venc.type = new_type;
-                if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-                        venc_power_off(dssdev);
-                        venc_power_on(dssdev);
-                }
+        if (out == NULL || out->manager == NULL) {
+                DSSERR("Failed to enable display: no output/manager\n");
+                r = -ENODEV;
+                goto err0;
         }
 
-        mutex_unlock(&venc.venc_lock);
-
-        return size;
-}
-
-static DEVICE_ATTR(output_type, S_IRUGO | S_IWUSR,
-                display_output_type_show, display_output_type_store);
-
-/* driver */
-static int venc_panel_probe(struct omap_dss_device *dssdev)
-{
-        dssdev->panel.timings = omap_dss_pal_timings;
-
-        return device_create_file(&dssdev->dev, &dev_attr_output_type);
-}
-
-static void venc_panel_remove(struct omap_dss_device *dssdev)
-{
-        device_remove_file(&dssdev->dev, &dev_attr_output_type);
-}
-
-static int venc_panel_enable(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        DSSDBG("venc_enable_display\n");
-
-        mutex_lock(&venc.venc_lock);
-
         r = omap_dss_start_device(dssdev);
         if (r) {
                 DSSERR("failed to start device\n");
                 goto err0;
         }
 
-        if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
-                r = -EINVAL;
-                goto err1;
-        }
+        if (dssdev->platform_enable)
+                dssdev->platform_enable(dssdev);
 
-        r = venc_runtime_get();
-        if (r)
-                goto err1;
 
         r = venc_power_on(dssdev);
         if (r)
-                goto err2;
+                goto err1;
 
         venc.wss_data = 0;
 
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
         mutex_unlock(&venc.venc_lock);
+
         return 0;
-err2:
-        venc_runtime_put();
 err1:
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
         omap_dss_stop_device(dssdev);
 err0:
         mutex_unlock(&venc.venc_lock);
-
         return r;
 }
 
-static void venc_panel_disable(struct omap_dss_device *dssdev)
+void omapdss_venc_display_disable(struct omap_dss_device *dssdev)
 {
-        DSSDBG("venc_disable_display\n");
+        DSSDBG("venc_display_disable\n");
 
         mutex_lock(&venc.venc_lock);
 
-        if (dssdev->state == OMAP_DSS_DISPLAY_DISABLED)
-                goto end;
-
-        if (dssdev->state == OMAP_DSS_DISPLAY_SUSPENDED) {
-                /* suspended is the same as disabled with venc */
-                dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-                goto end;
-        }
-
         venc_power_off(dssdev);
 
-        venc_runtime_put();
-
-        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-
         omap_dss_stop_device(dssdev);
-end:
-        mutex_unlock(&venc.venc_lock);
-}
 
-static int venc_panel_suspend(struct omap_dss_device *dssdev)
-{
-        venc_panel_disable(dssdev);
-        return 0;
-}
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
 
-static int venc_panel_resume(struct omap_dss_device *dssdev)
-{
-        return venc_panel_enable(dssdev);
+        mutex_unlock(&venc.venc_lock);
 }
 
-static void venc_set_timings(struct omap_dss_device *dssdev,
-                        struct omap_video_timings *timings)
+void omapdss_venc_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
 {
         DSSDBG("venc_set_timings\n");
 
+        mutex_lock(&venc.venc_lock);
+
         /* Reset WSS data when the TV standard changes. */
-        if (memcmp(&dssdev->panel.timings, timings, sizeof(*timings)))
+        if (memcmp(&venc.timings, timings, sizeof(*timings)))
                 venc.wss_data = 0;
 
-        dssdev->panel.timings = *timings;
-        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-                /* turn the venc off and on to get new timings to use */
-                venc_panel_disable(dssdev);
-                venc_panel_enable(dssdev);
-        } else {
-                dss_mgr_set_timings(dssdev->manager, timings);
-        }
+        venc.timings = *timings;
+
+        mutex_unlock(&venc.venc_lock);
 }
 
-static int venc_check_timings(struct omap_dss_device *dssdev,
-                        struct omap_video_timings *timings)
+int omapdss_venc_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
 {
         DSSDBG("venc_check_timings\n");
 
@@ -668,13 +587,13 @@ static int venc_check_timings(struct omap_dss_device *dssdev,
         return -EINVAL;
 }
 
-static u32 venc_get_wss(struct omap_dss_device *dssdev)
+u32 omapdss_venc_get_wss(struct omap_dss_device *dssdev)
 {
         /* Invert due to VENC_L21_WC_CTL:INV=1 */
         return (venc.wss_data >> 8) ^ 0xfffff;
 }
 
-static int venc_set_wss(struct omap_dss_device *dssdev, u32 wss)
+int omapdss_venc_set_wss(struct omap_dss_device *dssdev, u32 wss)
 {
         const struct venc_config *config;
         int r;
@@ -683,7 +602,7 @@ static int venc_set_wss(struct omap_dss_device *dssdev,	u32 wss)
 
         mutex_lock(&venc.venc_lock);
 
-        config = venc_timings_to_config(&dssdev->panel.timings);
+        config = venc_timings_to_config(&venc.timings);
 
         /* Invert due to VENC_L21_WC_CTL:INV=1 */
         venc.wss_data = (wss ^ 0xfffff) << 8;
@@ -703,30 +622,25 @@ err:
         return r;
 }
 
-static struct omap_dss_driver venc_driver = {
-        .probe          = venc_panel_probe,
-        .remove         = venc_panel_remove,
+void omapdss_venc_set_type(struct omap_dss_device *dssdev,
+                enum omap_dss_venc_type type)
+{
+        mutex_lock(&venc.venc_lock);
 
-        .enable         = venc_panel_enable,
-        .disable        = venc_panel_disable,
-        .suspend        = venc_panel_suspend,
-        .resume         = venc_panel_resume,
+        venc.type = type;
 
-        .get_resolution = omapdss_default_get_resolution,
-        .get_recommended_bpp = omapdss_default_get_recommended_bpp,
+        mutex_unlock(&venc.venc_lock);
+}
 
-        .set_timings    = venc_set_timings,
-        .check_timings  = venc_check_timings,
+void omapdss_venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
+                bool invert_polarity)
+{
+        mutex_lock(&venc.venc_lock);
 
-        .get_wss        = venc_get_wss,
-        .set_wss        = venc_set_wss,
+        venc.invert_polarity = invert_polarity;
 
-        .driver         = {
-                .name   = "venc",
-                .owner  = THIS_MODULE,
-        },
-};
-/* driver end */
+        mutex_unlock(&venc.venc_lock);
+}
 
 static int __init venc_init_display(struct omap_dss_device *dssdev)
 {
@@ -752,11 +666,6 @@ static void venc_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, venc_read_reg(r))
 
-        if (cpu_is_omap44xx()) {
-                seq_printf(s, "VENC currently disabled on OMAP44xx\n");
-                return;
-        }
-
         if (venc_runtime_get())
                 return;
 
@@ -832,10 +741,14 @@ static void venc_put_clocks(void)
                 clk_put(venc.tv_dac_clk);
 }
 
-static void __init venc_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init venc_find_dssdev(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int r, i;
+        const char *def_disp_name = dss_get_default_display_name();
+        struct omap_dss_device *def_dssdev;
+        int i;
+
+        def_dssdev = NULL;
 
         for (i = 0; i < pdata->num_devices; ++i) {
                 struct omap_dss_device *dssdev = pdata->devices[i];
@@ -843,17 +756,69 @@ static void __init venc_probe_pdata(struct platform_device *pdev)
                 if (dssdev->type != OMAP_DISPLAY_TYPE_VENC)
                         continue;
 
-                r = venc_init_display(dssdev);
-                if (r) {
-                        DSSERR("device %s init failed: %d\n", dssdev->name, r);
-                        continue;
+                if (def_dssdev == NULL)
+                        def_dssdev = dssdev;
+
+                if (def_disp_name != NULL &&
+                                strcmp(dssdev->name, def_disp_name) == 0) {
+                        def_dssdev = dssdev;
+                        break;
                 }
+        }
+
+        return def_dssdev;
+}
+
+static void __init venc_probe_pdata(struct platform_device *vencdev)
+{
+        struct omap_dss_device *plat_dssdev;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        plat_dssdev = venc_find_dssdev(vencdev);
 
-                r = omap_dss_register_device(dssdev, &pdev->dev, i);
-                if (r)
-                        DSSERR("device %s register failed: %d\n",
-                                        dssdev->name, r);
+        if (!plat_dssdev)
+                return;
+
+        dssdev = dss_alloc_and_init_device(&vencdev->dev);
+        if (!dssdev)
+                return;
+
+        dss_copy_device_pdata(dssdev, plat_dssdev);
+
+        dssdev->channel = OMAP_DSS_CHANNEL_DIGIT;
+
+        r = venc_init_display(dssdev);
+        if (r) {
+                DSSERR("device %s init failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
         }
+
+        r = dss_add_device(dssdev);
+        if (r) {
+                DSSERR("device %s register failed: %d\n", dssdev->name, r);
+                dss_put_device(dssdev);
+                return;
+        }
+}
+
+static void __init venc_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &venc.output;
+
+        out->pdev = pdev;
+        out->id = OMAP_DSS_OUTPUT_VENC;
+        out->type = OMAP_DISPLAY_TYPE_VENC;
+
+        dss_register_output(out);
+}
+
+static void __exit venc_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_output *out = &venc.output;
+
+        dss_unregister_output(out);
 }
 
 /* VENC HW IP initialisation */
@@ -897,17 +862,19 @@ static int __init omap_venchw_probe(struct platform_device *pdev)
 
         venc_runtime_put();
 
-        r = omap_dss_register_driver(&venc_driver);
+        r = venc_panel_init();
         if (r)
-                goto err_reg_panel_driver;
+                goto err_panel_init;
 
         dss_debugfs_create_file("venc", venc_dump_regs);
 
+        venc_init_output(pdev);
+
         venc_probe_pdata(pdev);
 
         return 0;
 
-err_reg_panel_driver:
+err_panel_init:
 err_runtime_get:
         pm_runtime_disable(&pdev->dev);
         venc_put_clocks();
@@ -916,14 +883,16 @@ err_runtime_get:
 
 static int __exit omap_venchw_remove(struct platform_device *pdev)
 {
-        omap_dss_unregister_child_devices(&pdev->dev);
+        dss_unregister_child_devices(&pdev->dev);
 
         if (venc.vdda_dac_reg != NULL) {
                 regulator_put(venc.vdda_dac_reg);
                 venc.vdda_dac_reg = NULL;
         }
 
-        omap_dss_unregister_driver(&venc_driver);
+        venc_panel_exit();
+
+        venc_uninit_output(pdev);
 
         pm_runtime_disable(&pdev->dev);
         venc_put_clocks();
@@ -971,16 +940,10 @@ static struct platform_driver omap_venchw_driver = {
 
 int __init venc_init_platform_driver(void)
 {
-        if (cpu_is_omap44xx())
-                return 0;
-
         return platform_driver_probe(&omap_venchw_driver, omap_venchw_probe);
 }
 
 void __exit venc_uninit_platform_driver(void)
 {
-        if (cpu_is_omap44xx())
-                return;
-
         platform_driver_unregister(&omap_venchw_driver);
 }
diff --git a/drivers/video/omap2/dss/venc_panel.c b/drivers/video/omap2/dss/venc_panel.c
new file mode 100644
index 000000000000..d55b8784ecfd
--- /dev/null
+++ b/drivers/video/omap2/dss/venc_panel.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * VENC panel driver
+ *
+ * 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 <video/omapdss.h>
+
+#include "dss.h"
+
+static struct {
+        struct mutex lock;
+} venc_panel;
+
+static ssize_t display_output_type_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        const char *ret;
+
+        switch (dssdev->phy.venc.type) {
+        case OMAP_DSS_VENC_TYPE_COMPOSITE:
+                ret = "composite";
+                break;
+        case OMAP_DSS_VENC_TYPE_SVIDEO:
+                ret = "svideo";
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return snprintf(buf, PAGE_SIZE, "%s\n", ret);
+}
+
+static ssize_t display_output_type_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t size)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        enum omap_dss_venc_type new_type;
+
+        if (sysfs_streq("composite", buf))
+                new_type = OMAP_DSS_VENC_TYPE_COMPOSITE;
+        else if (sysfs_streq("svideo", buf))
+                new_type = OMAP_DSS_VENC_TYPE_SVIDEO;
+        else
+                return -EINVAL;
+
+        mutex_lock(&venc_panel.lock);
+
+        if (dssdev->phy.venc.type != new_type) {
+                dssdev->phy.venc.type = new_type;
+                omapdss_venc_set_type(dssdev, new_type);
+                if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+                        omapdss_venc_display_disable(dssdev);
+                        omapdss_venc_display_enable(dssdev);
+                }
+        }
+
+        mutex_unlock(&venc_panel.lock);
+
+        return size;
+}
+
+static DEVICE_ATTR(output_type, S_IRUGO | S_IWUSR,
+                display_output_type_show, display_output_type_store);
+
+static int venc_panel_probe(struct omap_dss_device *dssdev)
+{
+        /* set default timings to PAL */
+        const struct omap_video_timings default_timings = {
+                .x_res          = 720,
+                .y_res          = 574,
+                .pixel_clock    = 13500,
+                .hsw            = 64,
+                .hfp            = 12,
+                .hbp            = 68,
+                .vsw            = 5,
+                .vfp            = 5,
+                .vbp            = 41,
+
+                .vsync_level    = OMAPDSS_SIG_ACTIVE_HIGH,
+                .hsync_level    = OMAPDSS_SIG_ACTIVE_HIGH,
+
+                .interlace      = true,
+        };
+
+        mutex_init(&venc_panel.lock);
+
+        dssdev->panel.timings = default_timings;
+
+        return device_create_file(&dssdev->dev, &dev_attr_output_type);
+}
+
+static void venc_panel_remove(struct omap_dss_device *dssdev)
+{
+        device_remove_file(&dssdev->dev, &dev_attr_output_type);
+}
+
+static int venc_panel_enable(struct omap_dss_device *dssdev)
+{
+        int r;
+
+        dev_dbg(&dssdev->dev, "venc_panel_enable\n");
+
+        mutex_lock(&venc_panel.lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        omapdss_venc_set_timings(dssdev, &dssdev->panel.timings);
+        omapdss_venc_set_type(dssdev, dssdev->phy.venc.type);
+        omapdss_venc_invert_vid_out_polarity(dssdev,
+                dssdev->phy.venc.invert_polarity);
+
+        r = omapdss_venc_display_enable(dssdev);
+        if (r)
+                goto err;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        mutex_unlock(&venc_panel.lock);
+
+        return 0;
+err:
+        mutex_unlock(&venc_panel.lock);
+
+        return r;
+}
+
+static void venc_panel_disable(struct omap_dss_device *dssdev)
+{
+        dev_dbg(&dssdev->dev, "venc_panel_disable\n");
+
+        mutex_lock(&venc_panel.lock);
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_DISABLED)
+                goto end;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_SUSPENDED) {
+                /* suspended is the same as disabled with venc */
+                dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+                goto end;
+        }
+
+        omapdss_venc_display_disable(dssdev);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+end:
+        mutex_unlock(&venc_panel.lock);
+}
+
+static int venc_panel_suspend(struct omap_dss_device *dssdev)
+{
+        venc_panel_disable(dssdev);
+        return 0;
+}
+
+static int venc_panel_resume(struct omap_dss_device *dssdev)
+{
+        return venc_panel_enable(dssdev);
+}
+
+static void venc_panel_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        dev_dbg(&dssdev->dev, "venc_panel_set_timings\n");
+
+        mutex_lock(&venc_panel.lock);
+
+        omapdss_venc_set_timings(dssdev, timings);
+        dssdev->panel.timings = *timings;
+
+        mutex_unlock(&venc_panel.lock);
+}
+
+static int venc_panel_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        dev_dbg(&dssdev->dev, "venc_panel_check_timings\n");
+
+        return omapdss_venc_check_timings(dssdev, timings);
+}
+
+static u32 venc_panel_get_wss(struct omap_dss_device *dssdev)
+{
+        dev_dbg(&dssdev->dev, "venc_panel_get_wss\n");
+
+        return omapdss_venc_get_wss(dssdev);
+}
+
+static int venc_panel_set_wss(struct omap_dss_device *dssdev, u32 wss)
+{
+        dev_dbg(&dssdev->dev, "venc_panel_set_wss\n");
+
+        return omapdss_venc_set_wss(dssdev, wss);
+}
+
+static struct omap_dss_driver venc_driver = {
+        .probe          = venc_panel_probe,
+        .remove         = venc_panel_remove,
+
+        .enable         = venc_panel_enable,
+        .disable        = venc_panel_disable,
+        .suspend        = venc_panel_suspend,
+        .resume         = venc_panel_resume,
+
+        .get_resolution = omapdss_default_get_resolution,
+        .get_recommended_bpp = omapdss_default_get_recommended_bpp,
+
+        .set_timings    = venc_panel_set_timings,
+        .check_timings  = venc_panel_check_timings,
+
+        .get_wss        = venc_panel_get_wss,
+        .set_wss        = venc_panel_set_wss,
+
+        .driver         = {
+                .name   = "venc",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int venc_panel_init(void)
+{
+        return omap_dss_register_driver(&venc_driver);
+}
+
+void venc_panel_exit(void)
+{
+        omap_dss_unregister_driver(&venc_driver);
+}
diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index c6cf372d22c5..606b89f12351 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -599,6 +599,7 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omap_dss_device *display = fb2display(fbi);
+        struct omap_overlay_manager *mgr;
 
         union {
                 struct omapfb_update_window_old uwnd_o;
@@ -786,12 +787,14 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
         case OMAPFB_WAITFORVSYNC:
                 DBG("ioctl WAITFORVSYNC\n");
-                if (!display) {
+                if (!display && !display->output && !display->output->manager) {
                         r = -EINVAL;
                         break;
                 }
 
-                r = display->manager->wait_for_vsync(display->manager);
+                mgr = display->output->manager;
+
+                r = mgr->wait_for_vsync(mgr);
                 break;
 
         case OMAPFB_WAITFORGO:
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 15373f4aee19..16db1589bd91 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -1593,6 +1593,20 @@ static int omapfb_allocate_all_fbs(struct omapfb2_device *fbdev)
         return 0;
 }
 
+static void omapfb_clear_fb(struct fb_info *fbi)
+{
+        const struct fb_fillrect rect = {
+                .dx = 0,
+                .dy = 0,
+                .width = fbi->var.xres_virtual,
+                .height = fbi->var.yres_virtual,
+                .color = 0,
+                .rop = ROP_COPY,
+        };
+
+        cfb_fillrect(fbi, &rect);
+}
+
 int omapfb_realloc_fbmem(struct fb_info *fbi, unsigned long size, int type)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
@@ -1662,6 +1676,8 @@ int omapfb_realloc_fbmem(struct fb_info *fbi, unsigned long size, int type)
                         goto err;
         }
 
+        omapfb_clear_fb(fbi);
+
         return 0;
 err:
         omapfb_free_fbmem(fbi);
@@ -1946,6 +1962,16 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
                 }
         }
 
+        for (i = 0; i < fbdev->num_fbs; i++) {
+                struct fb_info *fbi = fbdev->fbs[i];
+                struct omapfb_info *ofbi = FB2OFB(fbi);
+
+                if (ofbi->region->size == 0)
+                        continue;
+
+                omapfb_clear_fb(fbi);
+        }
+
         DBG("fb_infos initialized\n");
 
         for (i = 0; i < fbdev->num_fbs; i++) {
@@ -2354,6 +2380,7 @@ static int __init omapfb_probe(struct platform_device *pdev)
         struct omap_overlay *ovl;
         struct omap_dss_device *def_display;
         struct omap_dss_device *dssdev;
+        struct omap_dss_device *ovl_device;
 
         DBG("omapfb_probe\n");
 
@@ -2427,8 +2454,9 @@ static int __init omapfb_probe(struct platform_device *pdev)
         /* gfx overlay should be the default one. find a display
          * connected to that, and use it as default display */
         ovl = omap_dss_get_overlay(0);
-        if (ovl->manager && ovl->manager->device) {
-                def_display = ovl->manager->device;
+        ovl_device = ovl->get_device(ovl);
+        if (ovl_device) {
+                def_display = ovl_device;
         } else {
                 dev_warn(&pdev->dev, "cannot find default display\n");
                 def_display = NULL;
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index 30361a09aecd..5ced9b334d35 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -148,8 +148,9 @@ static inline struct omap_dss_device *fb2display(struct fb_info *fbi)
 
         /* XXX: returns the display connected to first attached overlay */
         for (i = 0; i < ofbi->num_overlays; i++) {
-                if (ofbi->overlays[i]->manager)
-                        return ofbi->overlays[i]->manager->device;
+                struct omap_overlay *ovl = ofbi->overlays[i];
+
+                return ovl->get_device(ovl);
         }
 
         return NULL;
diff --git a/drivers/video/omap2/vram.c b/drivers/video/omap2/vram.c
index 87e421e25afe..f2b15c4a75bc 100644
--- a/drivers/video/omap2/vram.c
+++ b/drivers/video/omap2/vram.c
@@ -34,7 +34,6 @@
 #include <asm/setup.h>
 
 #include <plat/vram.h>
-#include <plat/dma.h>
 
 #ifdef DEBUG
 #define DBG(format, ...) pr_debug("VRAM: " format, ## __VA_ARGS__)
@@ -250,59 +249,6 @@ int omap_vram_reserve(unsigned long paddr, size_t size)
 }
 EXPORT_SYMBOL(omap_vram_reserve);
 
-static void _omap_vram_dma_cb(int lch, u16 ch_status, void *data)
-{
-        struct completion *compl = data;
-        complete(compl);
-}
-
-static int _omap_vram_clear(u32 paddr, unsigned pages)
-{
-        struct completion compl;
-        unsigned elem_count;
-        unsigned frame_count;
-        int r;
-        int lch;
-
-        init_completion(&compl);
-
-        r = omap_request_dma(OMAP_DMA_NO_DEVICE, "VRAM DMA",
-                        _omap_vram_dma_cb,
-                        &compl, &lch);
-        if (r) {
-                pr_err("VRAM: request_dma failed for memory clear\n");
-                return -EBUSY;
-        }
-
-        elem_count = pages * PAGE_SIZE / 4;
-        frame_count = 1;
-
-        omap_set_dma_transfer_params(lch, OMAP_DMA_DATA_TYPE_S32,
-                        elem_count, frame_count,
-                        OMAP_DMA_SYNC_ELEMENT,
-                        0, 0);
-
-        omap_set_dma_dest_params(lch, 0, OMAP_DMA_AMODE_POST_INC,
-                        paddr, 0, 0);
-
-        omap_set_dma_color_mode(lch, OMAP_DMA_CONSTANT_FILL, 0x000000);
-
-        omap_start_dma(lch);
-
-        if (wait_for_completion_timeout(&compl, msecs_to_jiffies(1000)) == 0) {
-                omap_stop_dma(lch);
-                pr_err("VRAM: dma timeout while clearing memory\n");
-                r = -EIO;
-                goto err;
-        }
-
-        r = 0;
-err:
-        omap_free_dma(lch);
-
-        return r;
-}
-
 static int _omap_vram_alloc(unsigned pages, unsigned long *paddr)
 {
         struct vram_region *rm;
@@ -337,8 +283,6 @@ found:
 
                 *paddr = start;
 
-                _omap_vram_clear(start, pages);
-
                 return 0;
         }
 
diff --git a/drivers/video/pnx4008/Makefile b/drivers/video/pnx4008/Makefile
deleted file mode 100644
index 636aaccf01fd..000000000000
--- a/drivers/video/pnx4008/Makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-#
-# Makefile for the new PNX4008 framebuffer device driver
-#
-
-obj-$(CONFIG_FB_PNX4008_DUM) += sdum.o
-obj-$(CONFIG_FB_PNX4008_DUM_RGB) += pnxrgbfb.o
-
diff --git a/drivers/video/pnx4008/dum.h b/drivers/video/pnx4008/dum.h
deleted file mode 100644
index 1234d4375d92..000000000000
--- a/drivers/video/pnx4008/dum.h
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- * linux/drivers/video/pnx4008/dum.h
- *
- * Internal header for SDUM
- *
- * 2005 (c) Koninklijke Philips N.V. This file is licensed under
- * the terms of the GNU General Public License version 2. This program
- * is licensed "as is" without any warranty of any kind, whether express
- * or implied.
- */
-
-#ifndef __PNX008_DUM_H__
-#define __PNX008_DUM_H__
-
-#include <mach/platform.h>
-
-#define PNX4008_DUMCONF_VA_BASE         IO_ADDRESS(PNX4008_DUMCONF_BASE)
-#define PNX4008_DUM_MAIN_VA_BASE        IO_ADDRESS(PNX4008_DUM_MAINCFG_BASE)
-
-/* DUM CFG ADDRESSES */
-#define DUM_CH_BASE_ADR         (PNX4008_DUMCONF_VA_BASE + 0x00)
-#define DUM_CH_MIN_ADR          (PNX4008_DUMCONF_VA_BASE + 0x00)
-#define DUM_CH_MAX_ADR          (PNX4008_DUMCONF_VA_BASE + 0x04)
-#define DUM_CH_CONF_ADR         (PNX4008_DUMCONF_VA_BASE + 0x08)
-#define DUM_CH_STAT_ADR         (PNX4008_DUMCONF_VA_BASE + 0x0C)
-#define DUM_CH_CTRL_ADR         (PNX4008_DUMCONF_VA_BASE + 0x10)
-
-#define CH_MARG         (0x100 / sizeof(u32))
-#define DUM_CH_MIN(i)   (*((volatile u32 *)DUM_CH_MIN_ADR + (i) * CH_MARG))
-#define DUM_CH_MAX(i)   (*((volatile u32 *)DUM_CH_MAX_ADR + (i) * CH_MARG))
-#define DUM_CH_CONF(i)  (*((volatile u32 *)DUM_CH_CONF_ADR + (i) * CH_MARG))
-#define DUM_CH_STAT(i)  (*((volatile u32 *)DUM_CH_STAT_ADR + (i) * CH_MARG))
-#define DUM_CH_CTRL(i)  (*((volatile u32 *)DUM_CH_CTRL_ADR + (i) * CH_MARG))
-
-#define DUM_CONF_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x00)
-#define DUM_CTRL_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x04)
-#define DUM_STAT_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x08)
-#define DUM_DECODE_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x0C)
-#define DUM_COM_BASE_ADR      (PNX4008_DUM_MAIN_VA_BASE + 0x10)
-#define DUM_SYNC_C_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x14)
-#define DUM_CLK_DIV_ADR       (PNX4008_DUM_MAIN_VA_BASE + 0x18)
-#define DUM_DIRTY_LOW_ADR     (PNX4008_DUM_MAIN_VA_BASE + 0x20)
-#define DUM_DIRTY_HIGH_ADR    (PNX4008_DUM_MAIN_VA_BASE + 0x24)
-#define DUM_FORMAT_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x28)
-#define DUM_WTCFG1_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x30)
-#define DUM_RTCFG1_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x34)
-#define DUM_WTCFG2_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x38)
-#define DUM_RTCFG2_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x3C)
-#define DUM_TCFG_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x40)
-#define DUM_OUTP_FORMAT1_ADR  (PNX4008_DUM_MAIN_VA_BASE + 0x44)
-#define DUM_OUTP_FORMAT2_ADR  (PNX4008_DUM_MAIN_VA_BASE + 0x48)
-#define DUM_SYNC_MODE_ADR     (PNX4008_DUM_MAIN_VA_BASE + 0x4C)
-#define DUM_SYNC_OUT_C_ADR    (PNX4008_DUM_MAIN_VA_BASE + 0x50)
-
-#define DUM_CONF              (*(volatile u32 *)(DUM_CONF_ADR))
-#define DUM_CTRL              (*(volatile u32 *)(DUM_CTRL_ADR))
-#define DUM_STAT              (*(volatile u32 *)(DUM_STAT_ADR))
-#define DUM_DECODE            (*(volatile u32 *)(DUM_DECODE_ADR))
-#define DUM_COM_BASE          (*(volatile u32 *)(DUM_COM_BASE_ADR))
-#define DUM_SYNC_C            (*(volatile u32 *)(DUM_SYNC_C_ADR))
-#define DUM_CLK_DIV           (*(volatile u32 *)(DUM_CLK_DIV_ADR))
-#define DUM_DIRTY_LOW         (*(volatile u32 *)(DUM_DIRTY_LOW_ADR))
-#define DUM_DIRTY_HIGH        (*(volatile u32 *)(DUM_DIRTY_HIGH_ADR))
-#define DUM_FORMAT            (*(volatile u32 *)(DUM_FORMAT_ADR))
-#define DUM_WTCFG1            (*(volatile u32 *)(DUM_WTCFG1_ADR))
-#define DUM_RTCFG1            (*(volatile u32 *)(DUM_RTCFG1_ADR))
-#define DUM_WTCFG2            (*(volatile u32 *)(DUM_WTCFG2_ADR))
-#define DUM_RTCFG2            (*(volatile u32 *)(DUM_RTCFG2_ADR))
-#define DUM_TCFG              (*(volatile u32 *)(DUM_TCFG_ADR))
-#define DUM_OUTP_FORMAT1      (*(volatile u32 *)(DUM_OUTP_FORMAT1_ADR))
-#define DUM_OUTP_FORMAT2      (*(volatile u32 *)(DUM_OUTP_FORMAT2_ADR))
-#define DUM_SYNC_MODE         (*(volatile u32 *)(DUM_SYNC_MODE_ADR))
-#define DUM_SYNC_OUT_C        (*(volatile u32 *)(DUM_SYNC_OUT_C_ADR))
-
-/* DUM SLAVE ADDRESSES */
-#define DUM_SLAVE_WRITE_ADR      (PNX4008_DUM_MAINCFG_BASE + 0x0000000)
-#define DUM_SLAVE_READ1_I_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x1000000)
-#define DUM_SLAVE_READ1_R_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x1000004)
-#define DUM_SLAVE_READ2_I_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x1000008)
-#define DUM_SLAVE_READ2_R_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x100000C)
-
-#define DUM_SLAVE_WRITE_W  ((volatile u32 *)(DUM_SLAVE_WRITE_ADR))
-#define DUM_SLAVE_WRITE_HW ((volatile u16 *)(DUM_SLAVE_WRITE_ADR))
-#define DUM_SLAVE_READ1_I  ((volatile u8 *)(DUM_SLAVE_READ1_I_ADR))
-#define DUM_SLAVE_READ1_R  ((volatile u16 *)(DUM_SLAVE_READ1_R_ADR))
-#define DUM_SLAVE_READ2_I  ((volatile u8 *)(DUM_SLAVE_READ2_I_ADR))
-#define DUM_SLAVE_READ2_R  ((volatile u16 *)(DUM_SLAVE_READ2_R_ADR))
-
-/* Sony display register addresses */
-#define DISP_0_REG            (0x00)
-#define DISP_1_REG            (0x01)
-#define DISP_CAL_REG          (0x20)
-#define DISP_ID_REG           (0x2A)
-#define DISP_XMIN_L_REG       (0x30)
-#define DISP_XMIN_H_REG       (0x31)
-#define DISP_YMIN_REG         (0x32)
-#define DISP_XMAX_L_REG       (0x34)
-#define DISP_XMAX_H_REG       (0x35)
-#define DISP_YMAX_REG         (0x36)
-#define DISP_SYNC_EN_REG      (0x38)
-#define DISP_SYNC_RISE_L_REG  (0x3C)
-#define DISP_SYNC_RISE_H_REG  (0x3D)
-#define DISP_SYNC_FALL_L_REG  (0x3E)
-#define DISP_SYNC_FALL_H_REG  (0x3F)
-#define DISP_PIXEL_REG        (0x0B)
-#define DISP_DUMMY1_REG       (0x28)
-#define DISP_DUMMY2_REG       (0x29)
-#define DISP_TIMING_REG       (0x98)
-#define DISP_DUMP_REG         (0x99)
-
-/* Sony display constants */
-#define SONY_ID1              (0x22)
-#define SONY_ID2              (0x23)
-
-/* Philips display register addresses */
-#define PH_DISP_ORIENT_REG    (0x003)
-#define PH_DISP_YPOINT_REG    (0x200)
-#define PH_DISP_XPOINT_REG    (0x201)
-#define PH_DISP_PIXEL_REG     (0x202)
-#define PH_DISP_YMIN_REG      (0x406)
-#define PH_DISP_YMAX_REG      (0x407)
-#define PH_DISP_XMIN_REG      (0x408)
-#define PH_DISP_XMAX_REG      (0x409)
-
-/* Misc constants */
-#define NO_VALID_DISPLAY_FOUND      (0)
-#define DISPLAY2_IS_NOT_CONNECTED   (0)
-
-/* register values */
-#define V_BAC_ENABLE            (BIT(0))
-#define V_BAC_DISABLE_IDLE      (BIT(1))
-#define V_BAC_DISABLE_TRIG      (BIT(2))
-#define V_DUM_RESET             (BIT(3))
-#define V_MUX_RESET             (BIT(4))
-#define BAC_ENABLED             (BIT(0))
-#define BAC_DISABLED            0
-
-/* Sony LCD commands */
-#define V_LCD_STANDBY_OFF       ((BIT(25)) | (0 << 16) | DISP_0_REG)
-#define V_LCD_USE_9BIT_BUS      ((BIT(25)) | (2 << 16) | DISP_1_REG)
-#define V_LCD_SYNC_RISE_L       ((BIT(25)) | (0 << 16) | DISP_SYNC_RISE_L_REG)
-#define V_LCD_SYNC_RISE_H       ((BIT(25)) | (0 << 16) | DISP_SYNC_RISE_H_REG)
-#define V_LCD_SYNC_FALL_L       ((BIT(25)) | (160 << 16) | DISP_SYNC_FALL_L_REG)
-#define V_LCD_SYNC_FALL_H       ((BIT(25)) | (0 << 16) | DISP_SYNC_FALL_H_REG)
-#define V_LCD_SYNC_ENABLE       ((BIT(25)) | (128 << 16) | DISP_SYNC_EN_REG)
-#define V_LCD_DISPLAY_ON        ((BIT(25)) | (64 << 16) | DISP_0_REG)
-
-enum {
-        PAD_NONE,
-        PAD_512,
-        PAD_1024
-};
-
-enum {
-        RGB888,
-        RGB666,
-        RGB565,
-        BGR565,
-        ARGB1555,
-        ABGR1555,
-        ARGB4444,
-        ABGR4444
-};
-
-struct dum_setup {
-        int sync_neg_edge;
-        int round_robin;
-        int mux_int;
-        int synced_dirty_flag_int;
-        int dirty_flag_int;
-        int error_int;
-        int pf_empty_int;
-        int sf_empty_int;
-        int bac_dis_int;
-        u32 dirty_base_adr;
-        u32 command_base_adr;
-        u32 sync_clk_div;
-        int sync_output;
-        u32 sync_restart_val;
-        u32 set_sync_high;
-        u32 set_sync_low;
-};
-
-struct dum_ch_setup {
-        int disp_no;
-        u32 xmin;
-        u32 ymin;
-        u32 xmax;
-        u32 ymax;
-        int xmirror;
-        int ymirror;
-        int rotate;
-        u32 minadr;
-        u32 maxadr;
-        u32 dirtybuffer;
-        int pad;
-        int format;
-        int hwdirty;
-        int slave_trans;
-};
-
-struct disp_window {
-        u32 xmin_l;
-        u32 xmin_h;
-        u32 ymin;
-        u32 xmax_l;
-        u32 xmax_h;
-        u32 ymax;
-};
-
-#endif                          /* #ifndef __PNX008_DUM_H__ */
diff --git a/drivers/video/pnx4008/fbcommon.h b/drivers/video/pnx4008/fbcommon.h
deleted file mode 100644
index 4ebc87dafafb..000000000000
--- a/drivers/video/pnx4008/fbcommon.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright (C) 2005 Philips Semiconductors
- *
- * 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, 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; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA, or http://www.gnu.org/licenses/gpl.html
-*/
-
-#define QCIF_W  (176)
-#define QCIF_H  (144)
-
-#define CIF_W   (352)
-#define CIF_H   (288)
-
-#define LCD_X_RES       208
-#define LCD_Y_RES       320
-#define LCD_X_PAD       256
-#define LCD_BBP         4       /* Bytes Per Pixel */
-
-#define DISP_MAX_X_SIZE     (320)
-#define DISP_MAX_Y_SIZE     (208)
-
-#define RETURNVAL_BASE (0x400)
-
-enum fb_ioctl_returntype {
-        ENORESOURCESLEFT = RETURNVAL_BASE,
-        ERESOURCESNOTFREED,
-        EPROCNOTOWNER,
-        EFBNOTOWNER,
-        ECOPYFAILED,
-        EIOREMAPFAILED,
-};
diff --git a/drivers/video/pnx4008/pnxrgbfb.c b/drivers/video/pnx4008/pnxrgbfb.c
deleted file mode 100644
index 6d30428e9cf9..000000000000
--- a/drivers/video/pnx4008/pnxrgbfb.c
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * drivers/video/pnx4008/pnxrgbfb.c
- *
- * PNX4008's framebuffer support
- *
- * Author: Grigory Tolstolytkin <gtolstolytkin@ru.mvista.com>
- * Based on Philips Semiconductors's code
- *
- * Copyrght (c) 2005 MontaVista Software, Inc.
- * Copyright (c) 2005 Philips Semiconductors
- * This file is licensed under the terms of the GNU General Public License
- * version 2. This program is licensed "as is" without any warranty of any
- * kind, whether express or implied.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/vmalloc.h>
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/fb.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-
-#include "sdum.h"
-#include "fbcommon.h"
-
-static u32 colreg[16];
-
-static struct fb_var_screeninfo rgbfb_var __initdata = {
-        .xres = LCD_X_RES,
-        .yres = LCD_Y_RES,
-        .xres_virtual = LCD_X_RES,
-        .yres_virtual = LCD_Y_RES,
-        .bits_per_pixel = 32,
-        .red.offset = 16,
-        .red.length = 8,
-        .green.offset = 8,
-        .green.length = 8,
-        .blue.offset = 0,
-        .blue.length = 8,
-        .left_margin = 0,
-        .right_margin = 0,
-        .upper_margin = 0,
-        .lower_margin = 0,
-        .vmode = FB_VMODE_NONINTERLACED,
-};
-static struct fb_fix_screeninfo rgbfb_fix __initdata = {
-        .id = "RGBFB",
-        .line_length = LCD_X_RES * LCD_BBP,
-        .type = FB_TYPE_PACKED_PIXELS,
-        .visual = FB_VISUAL_TRUECOLOR,
-        .xpanstep = 0,
-        .ypanstep = 0,
-        .ywrapstep = 0,
-        .accel = FB_ACCEL_NONE,
-};
-
-static int channel_owned;
-
-static int no_cursor(struct fb_info *info, struct fb_cursor *cursor)
-{
-        return 0;
-}
-
-static int rgbfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
-                           u_int transp, struct fb_info *info)
-{
-        if (regno > 15)
-                return 1;
-
-        colreg[regno] = ((red & 0xff00) << 8) | (green & 0xff00) |
-            ((blue & 0xff00) >> 8);
-        return 0;
-}
-
-static int rgbfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
-{
-        return pnx4008_sdum_mmap(info, vma, NULL);
-}
-
-static struct fb_ops rgbfb_ops = {
-        .fb_mmap = rgbfb_mmap,
-        .fb_setcolreg = rgbfb_setcolreg,
-        .fb_fillrect = cfb_fillrect,
-        .fb_copyarea = cfb_copyarea,
-        .fb_imageblit = cfb_imageblit,
-};
-
-static int rgbfb_remove(struct platform_device *pdev)
-{
-        struct fb_info *info = platform_get_drvdata(pdev);
-
-        if (info) {
-                unregister_framebuffer(info);
-                fb_dealloc_cmap(&info->cmap);
-                framebuffer_release(info);
-                platform_set_drvdata(pdev, NULL);
-        }
-
-        pnx4008_free_dum_channel(channel_owned, pdev->id);
-        pnx4008_set_dum_exit_notification(pdev->id);
-
-        return 0;
-}
-
-static int __devinit rgbfb_probe(struct platform_device *pdev)
-{
-        struct fb_info *info;
-        struct dumchannel_uf chan_uf;
-        int ret;
-        char *option;
-
-        info = framebuffer_alloc(sizeof(u32) * 16, &pdev->dev);
-        if (!info) {
-                ret = -ENOMEM;
-                goto err;
-        }
-
-        pnx4008_get_fb_addresses(FB_TYPE_RGB, (void **)&info->screen_base,
-                                 (dma_addr_t *) &rgbfb_fix.smem_start,
-                                 &rgbfb_fix.smem_len);
-
-        if ((ret = pnx4008_alloc_dum_channel(pdev->id)) < 0)
-                goto err0;
-        else {
-                channel_owned = ret;
-                chan_uf.channelnr = channel_owned;
-                chan_uf.dirty = (u32 *) NULL;
-                chan_uf.source = (u32 *) rgbfb_fix.smem_start;
-                chan_uf.x_offset = 0;
-                chan_uf.y_offset = 0;
-                chan_uf.width = LCD_X_RES;
-                chan_uf.height = LCD_Y_RES;
-
-                if ((ret = pnx4008_put_dum_channel_uf(chan_uf, pdev->id))< 0)
-                        goto err1;
-
-                if ((ret =
-                     pnx4008_set_dum_channel_sync(channel_owned, CONF_SYNC_ON,
-                                                  pdev->id)) < 0)
-                        goto err1;
-
-                if ((ret =
-                     pnx4008_set_dum_channel_dirty_detect(channel_owned,
-                                                         CONF_DIRTYDETECTION_ON,
-                                                         pdev->id)) < 0)
-                        goto err1;
-        }
-
-        if (!fb_get_options("pnxrgbfb", &option) && option &&
-                        !strcmp(option, "nocursor"))
-                rgbfb_ops.fb_cursor = no_cursor;
-
-        info->node = -1;
-        info->flags = FBINFO_FLAG_DEFAULT;
-        info->fbops = &rgbfb_ops;
-        info->fix = rgbfb_fix;
-        info->var = rgbfb_var;
-        info->screen_size = rgbfb_fix.smem_len;
-        info->pseudo_palette = info->par;
-        info->par = NULL;
-
-        ret = fb_alloc_cmap(&info->cmap, 256, 0);
-        if (ret < 0)
-                goto err1;
-
-        ret = register_framebuffer(info);
-        if (ret < 0)
-                goto err2;
-        platform_set_drvdata(pdev, info);
-
-        return 0;
-
-err2:
-        fb_dealloc_cmap(&info->cmap);
-err1:
-        pnx4008_free_dum_channel(channel_owned, pdev->id);
-err0:
-        framebuffer_release(info);
-err:
-        return ret;
-}
-
-static struct platform_driver rgbfb_driver = {
-        .driver = {
-                .name = "pnx4008-rgbfb",
-        },
-        .probe = rgbfb_probe,
-        .remove = rgbfb_remove,
-};
-
-module_platform_driver(rgbfb_driver);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/pnx4008/sdum.c b/drivers/video/pnx4008/sdum.c
deleted file mode 100644
index c5c741452cac..000000000000
--- a/drivers/video/pnx4008/sdum.c
+++ /dev/null
@@ -1,861 +0,0 @@
-/*
- * drivers/video/pnx4008/sdum.c
- *
- * Display Update Master support
- *
- * Authors: Grigory Tolstolytkin <gtolstolytkin@ru.mvista.com>
- *          Vitaly Wool <vitalywool@gmail.com>
- * Based on Philips Semiconductors's code
- *
- * Copyrght (c) 2005-2006 MontaVista Software, Inc.
- * Copyright (c) 2005 Philips Semiconductors
- * This file is licensed under the terms of the GNU General Public License
- * version 2. This program is licensed "as is" without any warranty of any
- * kind, whether express or implied.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/tty.h>
-#include <linux/vmalloc.h>
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/platform_device.h>
-#include <linux/fb.h>
-#include <linux/init.h>
-#include <linux/dma-mapping.h>
-#include <linux/clk.h>
-#include <linux/gfp.h>
-#include <asm/uaccess.h>
-#include <asm/gpio.h>
-
-#include "sdum.h"
-#include "fbcommon.h"
-#include "dum.h"
-
-/* Framebuffers we have */
-
-static struct pnx4008_fb_addr {
-        int fb_type;
-        long addr_offset;
-        long fb_length;
-} fb_addr[] = {
-        [0] = {
-                FB_TYPE_YUV, 0, 0xB0000
-        },
-        [1] = {
-                FB_TYPE_RGB, 0xB0000, 0x50000
-        },
-};
-
-static struct dum_data {
-        u32 lcd_phys_start;
-        u32 lcd_virt_start;
-        u32 slave_phys_base;
-        u32 *slave_virt_base;
-        int fb_owning_channel[MAX_DUM_CHANNELS];
-        struct dumchannel_uf chan_uf_store[MAX_DUM_CHANNELS];
-} dum_data;
-
-/* Different local helper functions */
-
-static u32 nof_pixels_dx(struct dum_ch_setup *ch_setup)
-{
-        return (ch_setup->xmax - ch_setup->xmin + 1);
-}
-
-static u32 nof_pixels_dy(struct dum_ch_setup *ch_setup)
-{
-        return (ch_setup->ymax - ch_setup->ymin + 1);
-}
-
-static u32 nof_pixels_dxy(struct dum_ch_setup *ch_setup)
-{
-        return (nof_pixels_dx(ch_setup) * nof_pixels_dy(ch_setup));
-}
-
-static u32 nof_bytes(struct dum_ch_setup *ch_setup)
-{
-        u32 r = nof_pixels_dxy(ch_setup);
-        switch (ch_setup->format) {
-        case RGB888:
-        case RGB666:
-                r *= 4;
-                break;
-
-        default:
-                r *= 2;
-                break;
-        }
-        return r;
-}
-
-static u32 build_command(int disp_no, u32 reg, u32 val)
-{
-        return ((disp_no << 26) | BIT(25) | (val << 16) | (disp_no << 10) |
-                (reg << 0));
-}
-
-static u32 build_double_index(int disp_no, u32 val)
-{
-        return ((disp_no << 26) | (val << 16) | (disp_no << 10) | (val << 0));
-}
-
-static void build_disp_window(struct dum_ch_setup * ch_setup, struct disp_window * dw)
-{
-        dw->ymin = ch_setup->ymin;
-        dw->ymax = ch_setup->ymax;
-        dw->xmin_l = ch_setup->xmin & 0xFF;
-        dw->xmin_h = (ch_setup->xmin & BIT(8)) >> 8;
-        dw->xmax_l = ch_setup->xmax & 0xFF;
-        dw->xmax_h = (ch_setup->xmax & BIT(8)) >> 8;
-}
-
-static int put_channel(struct dumchannel chan)
-{
-        int i = chan.channelnr;
-
-        if (i < 0 || i > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else {
-                DUM_CH_MIN(i) = chan.dum_ch_min;
-                DUM_CH_MAX(i) = chan.dum_ch_max;
-                DUM_CH_CONF(i) = chan.dum_ch_conf;
-                DUM_CH_CTRL(i) = chan.dum_ch_ctrl;
-        }
-
-        return 0;
-}
-
-static void clear_channel(int channr)
-{
-        struct dumchannel chan;
-
-        chan.channelnr = channr;
-        chan.dum_ch_min = 0;
-        chan.dum_ch_max = 0;
-        chan.dum_ch_conf = 0;
-        chan.dum_ch_ctrl = 0;
-
-        put_channel(chan);
-}
-
-static int put_cmd_string(struct cmdstring cmds)
-{
-        u16 *cmd_str_virtaddr;
-        u32 *cmd_ptr0_virtaddr;
-        u32 cmd_str_physaddr;
-
-        int i = cmds.channelnr;
-
-        if (i < 0 || i > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if ((cmd_ptr0_virtaddr =
-                  (int *)ioremap_nocache(DUM_COM_BASE,
-                                         sizeof(int) * MAX_DUM_CHANNELS)) ==
-                 NULL)
-                return -EIOREMAPFAILED;
-        else {
-                cmd_str_physaddr = ioread32(&cmd_ptr0_virtaddr[cmds.channelnr]);
-                if ((cmd_str_virtaddr =
-                     (u16 *) ioremap_nocache(cmd_str_physaddr,
-                                             sizeof(cmds))) == NULL) {
-                        iounmap(cmd_ptr0_virtaddr);
-                        return -EIOREMAPFAILED;
-                } else {
-                        int t;
-                        for (t = 0; t < 8; t++)
-                                iowrite16(*((u16 *)&cmds.prestringlen + t),
-                                          cmd_str_virtaddr + t);
-
-                        for (t = 0; t < cmds.prestringlen / 2; t++)
-                                 iowrite16(*((u16 *)&cmds.precmd + t),
-                                           cmd_str_virtaddr + t + 8);
-
-                        for (t = 0; t < cmds.poststringlen / 2; t++)
-                                iowrite16(*((u16 *)&cmds.postcmd + t),
-                                          cmd_str_virtaddr + t + 8 +
-                                                cmds.prestringlen / 2);
-
-                        iounmap(cmd_ptr0_virtaddr);
-                        iounmap(cmd_str_virtaddr);
-                }
-        }
-
-        return 0;
-}
-
-static u32 dum_ch_setup(int ch_no, struct dum_ch_setup * ch_setup)
-{
-        struct cmdstring cmds_c;
-        struct cmdstring *cmds = &cmds_c;
-        struct disp_window dw;
-        int standard;
-        u32 orientation = 0;
-        struct dumchannel chan = { 0 };
-        int ret;
-
-        if ((ch_setup->xmirror) || (ch_setup->ymirror) || (ch_setup->rotate)) {
-                standard = 0;
-
-                orientation = BIT(1);   /* always set 9-bit-bus */
-                if (ch_setup->xmirror)
-                        orientation |= BIT(4);
-                if (ch_setup->ymirror)
-                        orientation |= BIT(3);
-                if (ch_setup->rotate)
-                        orientation |= BIT(0);
-        } else
-                standard = 1;
-
-        cmds->channelnr = ch_no;
-
-        /* build command string header */
-        if (standard) {
-                cmds->prestringlen = 32;
-                cmds->poststringlen = 0;
-        } else {
-                cmds->prestringlen = 48;
-                cmds->poststringlen = 16;
-        }
-
-        cmds->format =
-            (u16) ((ch_setup->disp_no << 4) | (BIT(3)) | (ch_setup->format));
-        cmds->reserved = 0x0;
-        cmds->startaddr_low = (ch_setup->minadr & 0xFFFF);
-        cmds->startaddr_high = (ch_setup->minadr >> 16);
-
-        if ((ch_setup->minadr == 0) && (ch_setup->maxadr == 0)
-            && (ch_setup->xmin == 0)
-            && (ch_setup->ymin == 0) && (ch_setup->xmax == 0)
-            && (ch_setup->ymax == 0)) {
-                cmds->pixdatlen_low = 0;
-                cmds->pixdatlen_high = 0;
-        } else {
-                u32 nbytes = nof_bytes(ch_setup);
-                cmds->pixdatlen_low = (nbytes & 0xFFFF);
-                cmds->pixdatlen_high = (nbytes >> 16);
-        }
-
-        if (ch_setup->slave_trans)
-                cmds->pixdatlen_high |= BIT(15);
-
-        /* build pre-string */
-        build_disp_window(ch_setup, &dw);
-
-        if (standard) {
-                cmds->precmd[0] =
-                    build_command(ch_setup->disp_no, DISP_XMIN_L_REG, 0x99);
-                cmds->precmd[1] =
-                    build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-                                  dw.xmin_l);
-                cmds->precmd[2] =
-                    build_command(ch_setup->disp_no, DISP_XMIN_H_REG,
-                                  dw.xmin_h);
-                cmds->precmd[3] =
-                    build_command(ch_setup->disp_no, DISP_YMIN_REG, dw.ymin);
-                cmds->precmd[4] =
-                    build_command(ch_setup->disp_no, DISP_XMAX_L_REG,
-                                  dw.xmax_l);
-                cmds->precmd[5] =
-                    build_command(ch_setup->disp_no, DISP_XMAX_H_REG,
-                                  dw.xmax_h);
-                cmds->precmd[6] =
-                    build_command(ch_setup->disp_no, DISP_YMAX_REG, dw.ymax);
-                cmds->precmd[7] =
-                    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-        } else {
-                if (dw.xmin_l == ch_no)
-                        cmds->precmd[0] =
-                            build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-                                          0x99);
-                else
-                        cmds->precmd[0] =
-                            build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-                                          ch_no);
-
-                cmds->precmd[1] =
-                    build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-                                  dw.xmin_l);
-                cmds->precmd[2] =
-                    build_command(ch_setup->disp_no, DISP_XMIN_H_REG,
-                                  dw.xmin_h);
-                cmds->precmd[3] =
-                    build_command(ch_setup->disp_no, DISP_YMIN_REG, dw.ymin);
-                cmds->precmd[4] =
-                    build_command(ch_setup->disp_no, DISP_XMAX_L_REG,
-                                  dw.xmax_l);
-                cmds->precmd[5] =
-                    build_command(ch_setup->disp_no, DISP_XMAX_H_REG,
-                                  dw.xmax_h);
-                cmds->precmd[6] =
-                    build_command(ch_setup->disp_no, DISP_YMAX_REG, dw.ymax);
-                cmds->precmd[7] =
-                    build_command(ch_setup->disp_no, DISP_1_REG, orientation);
-                cmds->precmd[8] =
-                    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-                cmds->precmd[9] =
-                    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-                cmds->precmd[0xA] =
-                    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-                cmds->precmd[0xB] =
-                    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-                cmds->postcmd[0] =
-                    build_command(ch_setup->disp_no, DISP_1_REG, BIT(1));
-                cmds->postcmd[1] =
-                    build_command(ch_setup->disp_no, DISP_DUMMY1_REG, 1);
-                cmds->postcmd[2] =
-                    build_command(ch_setup->disp_no, DISP_DUMMY1_REG, 2);
-                cmds->postcmd[3] =
-                    build_command(ch_setup->disp_no, DISP_DUMMY1_REG, 3);
-        }
-
-        if ((ret = put_cmd_string(cmds_c)) != 0) {
-                return ret;
-        }
-
-        chan.channelnr = cmds->channelnr;
-        chan.dum_ch_min = ch_setup->dirtybuffer + ch_setup->minadr;
-        chan.dum_ch_max = ch_setup->dirtybuffer + ch_setup->maxadr;
-        chan.dum_ch_conf = 0x002;
-        chan.dum_ch_ctrl = 0x04;
-
-        put_channel(chan);
-
-        return 0;
-}
-
-static u32 display_open(int ch_no, int auto_update, u32 * dirty_buffer,
-                        u32 * frame_buffer, u32 xpos, u32 ypos, u32 w, u32 h)
-{
-
-        struct dum_ch_setup k;
-        int ret;
-
-        /* keep width & height within display area */
-        if ((xpos + w) > DISP_MAX_X_SIZE)
-                w = DISP_MAX_X_SIZE - xpos;
-
-        if ((ypos + h) > DISP_MAX_Y_SIZE)
-                h = DISP_MAX_Y_SIZE - ypos;
-
-        /* assume 1 display only */
-        k.disp_no = 0;
-        k.xmin = xpos;
-        k.ymin = ypos;
-        k.xmax = xpos + (w - 1);
-        k.ymax = ypos + (h - 1);
-
-        /* adjust min and max values if necessary */
-        if (k.xmin > DISP_MAX_X_SIZE - 1)
-                k.xmin = DISP_MAX_X_SIZE - 1;
-        if (k.ymin > DISP_MAX_Y_SIZE - 1)
-                k.ymin = DISP_MAX_Y_SIZE - 1;
-
-        if (k.xmax > DISP_MAX_X_SIZE - 1)
-                k.xmax = DISP_MAX_X_SIZE - 1;
-        if (k.ymax > DISP_MAX_Y_SIZE - 1)
-                k.ymax = DISP_MAX_Y_SIZE - 1;
-
-        k.xmirror = 0;
-        k.ymirror = 0;
-        k.rotate = 0;
-        k.minadr = (u32) frame_buffer;
-        k.maxadr = (u32) frame_buffer + (((w - 1) << 10) | ((h << 2) - 2));
-        k.pad = PAD_1024;
-        k.dirtybuffer = (u32) dirty_buffer;
-        k.format = RGB888;
-        k.hwdirty = 0;
-        k.slave_trans = 0;
-
-        ret = dum_ch_setup(ch_no, &k);
-
-        return ret;
-}
-
-static void lcd_reset(void)
-{
-        u32 *dum_pio_base = (u32 *)IO_ADDRESS(PNX4008_PIO_BASE);
-
-        udelay(1);
-        iowrite32(BIT(19), &dum_pio_base[2]);
-        udelay(1);
-        iowrite32(BIT(19), &dum_pio_base[1]);
-        udelay(1);
-}
-
-static int dum_init(struct platform_device *pdev)
-{
-        struct clk *clk;
-
-        /* enable DUM clock */
-        clk = clk_get(&pdev->dev, "dum_ck");
-        if (IS_ERR(clk)) {
-                printk(KERN_ERR "pnx4008_dum: Unable to access DUM clock\n");
-                return PTR_ERR(clk);
-        }
-
-        clk_set_rate(clk, 1);
-        clk_put(clk);
-
-        DUM_CTRL = V_DUM_RESET;
-
-        /* set priority to "round-robin". All other params to "false" */
-        DUM_CONF = BIT(9);
-
-        /* Display 1 */
-        DUM_WTCFG1 = PNX4008_DUM_WT_CFG;
-        DUM_RTCFG1 = PNX4008_DUM_RT_CFG;
-        DUM_TCFG = PNX4008_DUM_T_CFG;
-
-        return 0;
-}
-
-static void dum_chan_init(void)
-{
-        int i = 0, ch = 0;
-        u32 *cmdptrs;
-        u32 *cmdstrings;
-
-        DUM_COM_BASE =
-                CMDSTRING_BASEADDR + BYTES_PER_CMDSTRING * NR_OF_CMDSTRINGS;
-
-        if ((cmdptrs =
-             (u32 *) ioremap_nocache(DUM_COM_BASE,
-                                     sizeof(u32) * NR_OF_CMDSTRINGS)) == NULL)
-                return;
-
-        for (ch = 0; ch < NR_OF_CMDSTRINGS; ch++)
-                iowrite32(CMDSTRING_BASEADDR + BYTES_PER_CMDSTRING * ch,
-                          cmdptrs + ch);
-
-        for (ch = 0; ch < MAX_DUM_CHANNELS; ch++)
-                clear_channel(ch);
-
-        /* Clear the cmdstrings */
-        cmdstrings =
-            (u32 *)ioremap_nocache(*cmdptrs,
-                                   BYTES_PER_CMDSTRING * NR_OF_CMDSTRINGS);
-
-        if (!cmdstrings)
-                goto out;
-
-        for (i = 0; i < NR_OF_CMDSTRINGS * BYTES_PER_CMDSTRING / sizeof(u32);
-             i++)
-                iowrite32(0, cmdstrings + i);
-
-        iounmap((u32 *)cmdstrings);
-
-out:
-        iounmap((u32 *)cmdptrs);
-}
-
-static void lcd_init(void)
-{
-        lcd_reset();
-
-        DUM_OUTP_FORMAT1 = 0; /* RGB666 */
-
-        udelay(1);
-        iowrite32(V_LCD_STANDBY_OFF, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_USE_9BIT_BUS, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_SYNC_RISE_L, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_SYNC_RISE_H, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_SYNC_FALL_L, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_SYNC_FALL_H, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_SYNC_ENABLE, dum_data.slave_virt_base);
-        udelay(1);
-        iowrite32(V_LCD_DISPLAY_ON, dum_data.slave_virt_base);
-        udelay(1);
-}
-
-/* Interface exported to framebuffer drivers */
-
-int pnx4008_get_fb_addresses(int fb_type, void **virt_addr,
-                             dma_addr_t *phys_addr, int *fb_length)
-{
-        int i;
-        int ret = -1;
-        for (i = 0; i < ARRAY_SIZE(fb_addr); i++)
-                if (fb_addr[i].fb_type == fb_type) {
-                        *virt_addr = (void *)(dum_data.lcd_virt_start +
-                                        fb_addr[i].addr_offset);
-                        *phys_addr =
-                            dum_data.lcd_phys_start + fb_addr[i].addr_offset;
-                        *fb_length = fb_addr[i].fb_length;
-                        ret = 0;
-                        break;
-                }
-
-        return ret;
-}
-
-EXPORT_SYMBOL(pnx4008_get_fb_addresses);
-
-int pnx4008_alloc_dum_channel(int dev_id)
-{
-        int i = 0;
-
-        while ((i < MAX_DUM_CHANNELS) && (dum_data.fb_owning_channel[i] != -1))
-                i++;
-
-        if (i == MAX_DUM_CHANNELS)
-                return -ENORESOURCESLEFT;
-        else {
-                dum_data.fb_owning_channel[i] = dev_id;
-                return i;
-        }
-}
-
-EXPORT_SYMBOL(pnx4008_alloc_dum_channel);
-
-int pnx4008_free_dum_channel(int channr, int dev_id)
-{
-        if (channr < 0 || channr > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if (dum_data.fb_owning_channel[channr] != dev_id)
-                return -EFBNOTOWNER;
-        else {
-                clear_channel(channr);
-                dum_data.fb_owning_channel[channr] = -1;
-        }
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_free_dum_channel);
-
-int pnx4008_put_dum_channel_uf(struct dumchannel_uf chan_uf, int dev_id)
-{
-        int i = chan_uf.channelnr;
-        int ret;
-
-        if (i < 0 || i > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if (dum_data.fb_owning_channel[i] != dev_id)
-                return -EFBNOTOWNER;
-        else if ((ret =
-                  display_open(chan_uf.channelnr, 0, chan_uf.dirty,
-                               chan_uf.source, chan_uf.y_offset,
-                               chan_uf.x_offset, chan_uf.height,
-                               chan_uf.width)) != 0)
-                return ret;
-        else {
-                dum_data.chan_uf_store[i].dirty = chan_uf.dirty;
-                dum_data.chan_uf_store[i].source = chan_uf.source;
-                dum_data.chan_uf_store[i].x_offset = chan_uf.x_offset;
-                dum_data.chan_uf_store[i].y_offset = chan_uf.y_offset;
-                dum_data.chan_uf_store[i].width = chan_uf.width;
-                dum_data.chan_uf_store[i].height = chan_uf.height;
-        }
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_put_dum_channel_uf);
-
-int pnx4008_set_dum_channel_sync(int channr, int val, int dev_id)
-{
-        if (channr < 0 || channr > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if (dum_data.fb_owning_channel[channr] != dev_id)
-                return -EFBNOTOWNER;
-        else {
-                if (val == CONF_SYNC_ON) {
-                        DUM_CH_CONF(channr) |= CONF_SYNCENABLE;
-                        DUM_CH_CONF(channr) |= DUM_CHANNEL_CFG_SYNC_MASK |
-                                DUM_CHANNEL_CFG_SYNC_MASK_SET;
-                } else if (val == CONF_SYNC_OFF)
-                        DUM_CH_CONF(channr) &= ~CONF_SYNCENABLE;
-                else
-                        return -EINVAL;
-        }
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_set_dum_channel_sync);
-
-int pnx4008_set_dum_channel_dirty_detect(int channr, int val, int dev_id)
-{
-        if (channr < 0 || channr > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if (dum_data.fb_owning_channel[channr] != dev_id)
-                return -EFBNOTOWNER;
-        else {
-                if (val == CONF_DIRTYDETECTION_ON)
-                        DUM_CH_CONF(channr) |= CONF_DIRTYENABLE;
-                else if (val == CONF_DIRTYDETECTION_OFF)
-                        DUM_CH_CONF(channr) &= ~CONF_DIRTYENABLE;
-                else
-                        return -EINVAL;
-        }
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_set_dum_channel_dirty_detect);
-
-#if 0 /* Functions not used currently, but likely to be used in future */
-
-static int get_channel(struct dumchannel *p_chan)
-{
-        int i = p_chan->channelnr;
-
-        if (i < 0 || i > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else {
-                p_chan->dum_ch_min = DUM_CH_MIN(i);
-                p_chan->dum_ch_max = DUM_CH_MAX(i);
-                p_chan->dum_ch_conf = DUM_CH_CONF(i);
-                p_chan->dum_ch_stat = DUM_CH_STAT(i);
-                p_chan->dum_ch_ctrl = 0;        /* WriteOnly control register */
-        }
-
-        return 0;
-}
-
-int pnx4008_get_dum_channel_uf(struct dumchannel_uf *p_chan_uf, int dev_id)
-{
-        int i = p_chan_uf->channelnr;
-
-        if (i < 0 || i > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if (dum_data.fb_owning_channel[i] != dev_id)
-                return -EFBNOTOWNER;
-        else {
-                p_chan_uf->dirty = dum_data.chan_uf_store[i].dirty;
-                p_chan_uf->source = dum_data.chan_uf_store[i].source;
-                p_chan_uf->x_offset = dum_data.chan_uf_store[i].x_offset;
-                p_chan_uf->y_offset = dum_data.chan_uf_store[i].y_offset;
-                p_chan_uf->width = dum_data.chan_uf_store[i].width;
-                p_chan_uf->height = dum_data.chan_uf_store[i].height;
-        }
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_get_dum_channel_uf);
-
-int pnx4008_get_dum_channel_config(int channr, int dev_id)
-{
-        int ret;
-        struct dumchannel chan;
-
-        if (channr < 0 || channr > MAX_DUM_CHANNELS)
-                return -EINVAL;
-        else if (dum_data.fb_owning_channel[channr] != dev_id)
-                return -EFBNOTOWNER;
-        else {
-                chan.channelnr = channr;
-                if ((ret = get_channel(&chan)) != 0)
-                        return ret;
-        }
-
-        return (chan.dum_ch_conf & DUM_CHANNEL_CFG_MASK);
-}
-
-EXPORT_SYMBOL(pnx4008_get_dum_channel_config);
-
-int pnx4008_force_update_dum_channel(int channr, int dev_id)
-{
-        if (channr < 0 || channr > MAX_DUM_CHANNELS)
-                return -EINVAL;
-
-        else if (dum_data.fb_owning_channel[channr] != dev_id)
-                return -EFBNOTOWNER;
-        else
-                DUM_CH_CTRL(channr) = CTRL_SETDIRTY;
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_force_update_dum_channel);
-
-#endif
-
-int pnx4008_sdum_mmap(struct fb_info *info, struct vm_area_struct *vma,
-                      struct device *dev)
-{
-        unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
-
-        if (off < info->fix.smem_len) {
-                vma->vm_pgoff += 1;
-                return dma_mmap_writecombine(dev, vma,
-                                (void *)dum_data.lcd_virt_start,
-                                dum_data.lcd_phys_start,
-                                FB_DMA_SIZE);
-        }
-        return -EINVAL;
-}
-
-EXPORT_SYMBOL(pnx4008_sdum_mmap);
-
-int pnx4008_set_dum_exit_notification(int dev_id)
-{
-        int i;
-
-        for (i = 0; i < MAX_DUM_CHANNELS; i++)
-                if (dum_data.fb_owning_channel[i] == dev_id)
-                        return -ERESOURCESNOTFREED;
-
-        return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_set_dum_exit_notification);
-
-/* Platform device driver for DUM */
-
-static int sdum_suspend(struct platform_device *pdev, pm_message_t state)
-{
-        int retval = 0;
-        struct clk *clk;
-
-        clk = clk_get(0, "dum_ck");
-        if (!IS_ERR(clk)) {
-                clk_set_rate(clk, 0);
-                clk_put(clk);
-        } else
-                retval = PTR_ERR(clk);
-
-        /* disable BAC */
-        DUM_CTRL = V_BAC_DISABLE_IDLE;
-
-        /* LCD standby & turn off display */
-        lcd_reset();
-
-        return retval;
-}
-
-static int sdum_resume(struct platform_device *pdev)
-{
-        int retval = 0;
-        struct clk *clk;
-
-        clk = clk_get(0, "dum_ck");
-        if (!IS_ERR(clk)) {
-                clk_set_rate(clk, 1);
-                clk_put(clk);
-        } else
-                retval = PTR_ERR(clk);
-
-        /* wait for BAC disable */
-        DUM_CTRL = V_BAC_DISABLE_TRIG;
-
-        while (DUM_CTRL & BAC_ENABLED)
-                udelay(10);
-
-        /* re-init LCD */
-        lcd_init();
-
-        /* enable BAC and reset MUX */
-        DUM_CTRL = V_BAC_ENABLE;
-        udelay(1);
-        DUM_CTRL = V_MUX_RESET;
-        return 0;
-}
-
-static int __devinit sdum_probe(struct platform_device *pdev)
-{
-        int ret = 0, i = 0;
-
-        /* map frame buffer */
-        dum_data.lcd_virt_start = (u32) dma_alloc_writecombine(&pdev->dev,
-                                                       FB_DMA_SIZE,
-                                                       &dum_data.lcd_phys_start,
-                                                       GFP_KERNEL);
-
-        if (!dum_data.lcd_virt_start) {
-                ret = -ENOMEM;
-                goto out_3;
-        }
-
-        /* map slave registers */
-        dum_data.slave_phys_base = PNX4008_DUM_SLAVE_BASE;
-        dum_data.slave_virt_base =
-            (u32 *) ioremap_nocache(dum_data.slave_phys_base, sizeof(u32));
-
-        if (dum_data.slave_virt_base == NULL) {
-                ret = -ENOMEM;
-                goto out_2;
-        }
-
-        /* initialize DUM and LCD display */
-        ret = dum_init(pdev);
-        if (ret)
-                goto out_1;
-
-        dum_chan_init();
-        lcd_init();
-
-        DUM_CTRL = V_BAC_ENABLE;
-        udelay(1);
-        DUM_CTRL = V_MUX_RESET;
-
-        /* set decode address and sync clock divider */
-        DUM_DECODE = dum_data.lcd_phys_start & DUM_DECODE_MASK;
-        DUM_CLK_DIV = PNX4008_DUM_CLK_DIV;
-
-        for (i = 0; i < MAX_DUM_CHANNELS; i++)
-                dum_data.fb_owning_channel[i] = -1;
-
-        /*setup wakeup interrupt */
-        start_int_set_rising_edge(SE_DISP_SYNC_INT);
-        start_int_ack(SE_DISP_SYNC_INT);
-        start_int_umask(SE_DISP_SYNC_INT);
-
-        return 0;
-
-out_1:
-        iounmap((void *)dum_data.slave_virt_base);
-out_2:
-        dma_free_writecombine(&pdev->dev, FB_DMA_SIZE,
-                        (void *)dum_data.lcd_virt_start,
-                        dum_data.lcd_phys_start);
-out_3:
-        return ret;
-}
-
-static int sdum_remove(struct platform_device *pdev)
-{
-        struct clk *clk;
-
-        start_int_mask(SE_DISP_SYNC_INT);
-
-        clk = clk_get(0, "dum_ck");
-        if (!IS_ERR(clk)) {
-                clk_set_rate(clk, 0);
-                clk_put(clk);
-        }
-
-        iounmap((void *)dum_data.slave_virt_base);
-
-        dma_free_writecombine(&pdev->dev, FB_DMA_SIZE,
-                        (void *)dum_data.lcd_virt_start,
-                        dum_data.lcd_phys_start);
-
-        return 0;
-}
-
-static struct platform_driver sdum_driver = {
-        .driver = {
-                .name = "pnx4008-sdum",
-        },
-        .probe = sdum_probe,
-        .remove = sdum_remove,
-        .suspend = sdum_suspend,
-        .resume = sdum_resume,
-};
-
-module_platform_driver(sdum_driver);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/pnx4008/sdum.h b/drivers/video/pnx4008/sdum.h
deleted file mode 100644
index 189c3d641383..000000000000
--- a/drivers/video/pnx4008/sdum.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * Copyright (C) 2005 Philips Semiconductors
- *
- * 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, 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; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA, or http://www.gnu.org/licenses/gpl.html
-*/
-
-#define MAX_DUM_CHANNELS        64
-
-#define RGB_MEM_WINDOW(x) (0x10000000 + (x)*0x00100000)
-
-#define QCIF_OFFSET(x) (((x) == 0) ? 0x00000: ((x) == 1) ? 0x30000: -1)
-#define CIF_OFFSET(x)  (((x) == 0) ? 0x00000: ((x) == 1) ? 0x60000: -1)
-
-#define CTRL_SETDIRTY           (0x00000001)
-#define CONF_DIRTYENABLE        (0x00000020)
-#define CONF_SYNCENABLE         (0x00000004)
-
-#define DIRTY_ENABLED(conf)     ((conf) & 0x0020)
-#define SYNC_ENABLED(conf)      ((conf) & 0x0004)
-
-/* Display 1 & 2 Write Timing Configuration */
-#define PNX4008_DUM_WT_CFG              0x00372000
-
-/* Display 1 & 2 Read Timing Configuration */
-#define PNX4008_DUM_RT_CFG              0x00003A47
-
-/* DUM Transit State Timing Configuration */
-#define PNX4008_DUM_T_CFG               0x1D    /* 29 HCLK cycles */
-
-/* DUM Sync count clock divider */
-#define PNX4008_DUM_CLK_DIV             0x02DD
-
-/* Memory size for framebuffer, allocated through dma_alloc_writecombine().
- * Must be PAGE aligned
- */
-#define FB_DMA_SIZE (PAGE_ALIGN(SZ_1M + PAGE_SIZE))
-
-#define OFFSET_RGBBUFFER (0xB0000)
-#define OFFSET_YUVBUFFER (0x00000)
-
-#define YUVBUFFER (lcd_video_start + OFFSET_YUVBUFFER)
-#define RGBBUFFER (lcd_video_start + OFFSET_RGBBUFFER)
-
-#define CMDSTRING_BASEADDR      (0x00C000)      /* iram */
-#define BYTES_PER_CMDSTRING     (0x80)
-#define NR_OF_CMDSTRINGS        (64)
-
-#define MAX_NR_PRESTRINGS (0x40)
-#define MAX_NR_POSTSTRINGS (0x40)
-
-/* various mask definitions */
-#define DUM_CLK_ENABLE 0x01
-#define DUM_CLK_DISABLE 0
-#define DUM_DECODE_MASK 0x1FFFFFFF
-#define DUM_CHANNEL_CFG_MASK 0x01FF
-#define DUM_CHANNEL_CFG_SYNC_MASK 0xFFFE00FF
-#define DUM_CHANNEL_CFG_SYNC_MASK_SET 0x0CA00
-
-#define SDUM_RETURNVAL_BASE (0x500)
-
-#define CONF_SYNC_OFF           (0x602)
-#define CONF_SYNC_ON            (0x603)
-
-#define CONF_DIRTYDETECTION_OFF (0x600)
-#define CONF_DIRTYDETECTION_ON  (0x601)
-
-struct dumchannel_uf {
-        int channelnr;
-        u32 *dirty;
-        u32 *source;
-        u32 x_offset;
-        u32 y_offset;
-        u32 width;
-        u32 height;
-};
-
-enum {
-        FB_TYPE_YUV,
-        FB_TYPE_RGB
-};
-
-struct cmdstring {
-        int channelnr;
-        uint16_t prestringlen;
-        uint16_t poststringlen;
-        uint16_t format;
-        uint16_t reserved;
-        uint16_t startaddr_low;
-        uint16_t startaddr_high;
-        uint16_t pixdatlen_low;
-        uint16_t pixdatlen_high;
-        u32 precmd[MAX_NR_PRESTRINGS];
-        u32 postcmd[MAX_NR_POSTSTRINGS];
-
-};
-
-struct dumchannel {
-        int channelnr;
-        int dum_ch_min;
-        int dum_ch_max;
-        int dum_ch_conf;
-        int dum_ch_stat;
-        int dum_ch_ctrl;
-};
-
-int pnx4008_alloc_dum_channel(int dev_id);
-int pnx4008_free_dum_channel(int channr, int dev_id);
-
-int pnx4008_get_dum_channel_uf(struct dumchannel_uf *pChan_uf, int dev_id);
-int pnx4008_put_dum_channel_uf(struct dumchannel_uf chan_uf, int dev_id);
-
-int pnx4008_set_dum_channel_sync(int channr, int val, int dev_id);
-int pnx4008_set_dum_channel_dirty_detect(int channr, int val, int dev_id);
-
-int pnx4008_force_dum_update_channel(int channr, int dev_id);
-
-int pnx4008_get_dum_channel_config(int channr, int dev_id);
-
-int pnx4008_sdum_mmap(struct fb_info *info, struct vm_area_struct *vma, struct device *dev);
-int pnx4008_set_dum_exit_notification(int dev_id);
-
-int pnx4008_get_fb_addresses(int fb_type, void **virt_addr,
-                             dma_addr_t * phys_addr, int *fb_length);
diff --git a/drivers/video/ps3fb.c b/drivers/video/ps3fb.c
index 4e292f29bf5d..0b340d6ff8a4 100644
--- a/drivers/video/ps3fb.c
+++ b/drivers/video/ps3fb.c
@@ -1034,6 +1034,7 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
         if (status) {
                 dev_err(&dev->core, "%s: lv1_gpu_memory_allocate failed: %d\n",
                         __func__, status);
+                retval = -ENOMEM;
                 goto err_close_device;
         }
         dev_dbg(&dev->core, "ddr:lpar:0x%llx\n", ddr_lpar);
@@ -1046,6 +1047,7 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
                 dev_err(&dev->core,
                         "%s: lv1_gpu_context_allocate failed: %d\n", __func__,
                         status);
+                retval = -ENOMEM;
                 goto err_gpu_memory_free;
         }
 
@@ -1053,6 +1055,7 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
         dinfo = (void __force *)ioremap(lpar_driver_info, 128 * 1024);
         if (!dinfo) {
                 dev_err(&dev->core, "%s: ioremap failed\n", __func__);
+                retval = -ENOMEM;
                 goto err_gpu_context_free;
         }
 
@@ -1121,8 +1124,10 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
         }
 
         info = framebuffer_alloc(sizeof(struct ps3fb_par), &dev->core);
-        if (!info)
+        if (!info) {
+                retval = -ENOMEM;
                 goto err_context_fb_close;
+        }
 
         par = info->par;
         par->mode_id = ~ps3fb_mode;     /* != ps3fb_mode, to trigger change */
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index 69bf9d07c237..2ed7b633bbd9 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -25,8 +25,8 @@
 #include <linux/interrupt.h>
 #include <linux/pm_runtime.h>
 
+#include <video/samsung_fimd.h>
 #include <mach/map.h>
-#include <plat/regs-fb-v4.h>
 #include <plat/fb.h>
 
 /* This driver will export a number of framebuffer interfaces depending
@@ -1398,35 +1398,28 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 
         spin_lock_init(&sfb->slock);
 
-        sfb->bus_clk = clk_get(dev, "lcd");
+        sfb->bus_clk = devm_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;
+                return PTR_ERR(sfb->bus_clk);
         }
 
-        clk_enable(sfb->bus_clk);
+        clk_prepare_enable(sfb->bus_clk);
 
         if (!sfb->variant.has_clksel) {
-                sfb->lcd_clk = clk_get(dev, "sclk_fimd");
+                sfb->lcd_clk = devm_clk_get(dev, "sclk_fimd");
                 if (IS_ERR(sfb->lcd_clk)) {
                         dev_err(dev, "failed to get lcd clock\n");
                         ret = PTR_ERR(sfb->lcd_clk);
                         goto err_bus_clk;
                 }
 
-                clk_enable(sfb->lcd_clk);
+                clk_prepare_enable(sfb->lcd_clk);
         }
 
         pm_runtime_enable(sfb->dev);
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        if (!res) {
-                dev_err(dev, "failed to find registers\n");
-                ret = -ENOENT;
-                goto err_lcd_clk;
-        }
-
         sfb->regs = devm_request_and_ioremap(dev, res);
         if (!sfb->regs) {
                 dev_err(dev, "failed to map registers\n");
@@ -1510,16 +1503,12 @@ err_pm_runtime:
 err_lcd_clk:
         pm_runtime_disable(sfb->dev);
 
-        if (!sfb->variant.has_clksel) {
-                clk_disable(sfb->lcd_clk);
-                clk_put(sfb->lcd_clk);
-        }
+        if (!sfb->variant.has_clksel)
+                clk_disable_unprepare(sfb->lcd_clk);
 
 err_bus_clk:
-        clk_disable(sfb->bus_clk);
-        clk_put(sfb->bus_clk);
+        clk_disable_unprepare(sfb->bus_clk);
 
-err_sfb:
         return ret;
 }
 
@@ -1541,13 +1530,10 @@ static int __devexit s3c_fb_remove(struct platform_device *pdev)
                 if (sfb->windows[win])
                         s3c_fb_release_win(sfb, sfb->windows[win]);
 
-        if (!sfb->variant.has_clksel) {
-                clk_disable(sfb->lcd_clk);
-                clk_put(sfb->lcd_clk);
-        }
+        if (!sfb->variant.has_clksel)
+                clk_disable_unprepare(sfb->lcd_clk);
 
-        clk_disable(sfb->bus_clk);
-        clk_put(sfb->bus_clk);
+        clk_disable_unprepare(sfb->bus_clk);
 
         pm_runtime_put_sync(sfb->dev);
         pm_runtime_disable(sfb->dev);
@@ -1575,9 +1561,9 @@ static int s3c_fb_suspend(struct device *dev)
         }
 
         if (!sfb->variant.has_clksel)
-                clk_disable(sfb->lcd_clk);
+                clk_disable_unprepare(sfb->lcd_clk);
 
-        clk_disable(sfb->bus_clk);
+        clk_disable_unprepare(sfb->bus_clk);
 
         pm_runtime_put_sync(sfb->dev);
 
@@ -1595,10 +1581,10 @@ static int s3c_fb_resume(struct device *dev)
 
         pm_runtime_get_sync(sfb->dev);
 
-        clk_enable(sfb->bus_clk);
+        clk_prepare_enable(sfb->bus_clk);
 
         if (!sfb->variant.has_clksel)
-                clk_enable(sfb->lcd_clk);
+                clk_prepare_enable(sfb->lcd_clk);
 
         /* setup gpio and output polarity controls */
         pd->setup_gpio();
@@ -1654,9 +1640,9 @@ static int s3c_fb_runtime_suspend(struct device *dev)
         struct s3c_fb *sfb = platform_get_drvdata(pdev);
 
         if (!sfb->variant.has_clksel)
-                clk_disable(sfb->lcd_clk);
+                clk_disable_unprepare(sfb->lcd_clk);
 
-        clk_disable(sfb->bus_clk);
+        clk_disable_unprepare(sfb->bus_clk);
 
         return 0;
 }
@@ -1667,10 +1653,10 @@ static int s3c_fb_runtime_resume(struct device *dev)
         struct s3c_fb *sfb = platform_get_drvdata(pdev);
         struct s3c_fb_platdata *pd = sfb->pdata;
 
-        clk_enable(sfb->bus_clk);
+        clk_prepare_enable(sfb->bus_clk);
 
         if (!sfb->variant.has_clksel)
-                clk_enable(sfb->lcd_clk);
+                clk_prepare_enable(sfb->lcd_clk);
 
         /* setup gpio and output polarity controls */
         pd->setup_gpio();
diff --git a/drivers/video/s3c2410fb.c b/drivers/video/s3c2410fb.c
index 77f34c614c86..1083bb9469ee 100644
--- a/drivers/video/s3c2410fb.c
+++ b/drivers/video/s3c2410fb.c
@@ -11,6 +11,8 @@
  * Driver based on skeletonfb.c, sa1100fb.c and others.
 */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/err.h>
@@ -48,7 +50,11 @@ static int debug	= 1;
 static int debug;
 #endif
 
-#define dprintk(msg...) if (debug) printk(KERN_DEBUG "s3c2410fb: " msg);
+#define dprintk(msg...) \
+do { \
+        if (debug) \
+                pr_debug(msg); \
+} while (0)
 
 /* useful functions */
 
@@ -598,11 +604,11 @@ static int s3c2410fb_debug_store(struct device *dev,
         if (strnicmp(buf, "on", 2) == 0 ||
             strnicmp(buf, "1", 1) == 0) {
                 debug = 1;
-                printk(KERN_DEBUG "s3c2410fb: Debug On");
+                dev_dbg(dev, "s3c2410fb: Debug On");
         } else if (strnicmp(buf, "off", 3) == 0 ||
                    strnicmp(buf, "0", 1) == 0) {
                 debug = 0;
-                printk(KERN_DEBUG "s3c2410fb: Debug Off");
+                dev_dbg(dev, "s3c2410fb: Debug Off");
         } else {
                 return -EINVAL;
         }
@@ -921,7 +927,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 
         info->clk = clk_get(NULL, "lcd");
         if (IS_ERR(info->clk)) {
-                printk(KERN_ERR "failed to get lcd clock source\n");
+                dev_err(&pdev->dev, "failed to get lcd clock source\n");
                 ret = PTR_ERR(info->clk);
                 goto release_irq;
         }
@@ -929,7 +935,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
         clk_enable(info->clk);
         dprintk("got and enabled clock\n");
 
-        usleep_range(1000, 1000);
+        usleep_range(1000, 1100);
 
         info->clk_rate = clk_get_rate(info->clk);
 
@@ -947,7 +953,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
         /* Initialize video memory */
         ret = s3c2410fb_map_video_memory(fbinfo);
         if (ret) {
-                printk(KERN_ERR "Failed to allocate video RAM: %d\n", ret);
+                dev_err(&pdev->dev, "Failed to allocate video RAM: %d\n", ret);
                 ret = -ENOMEM;
                 goto release_clock;
         }
@@ -970,7 +976,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 
         ret = register_framebuffer(fbinfo);
         if (ret < 0) {
-                printk(KERN_ERR "Failed to register framebuffer device: %d\n",
+                dev_err(&pdev->dev, "Failed to register framebuffer device: %d\n",
                         ret);
                 goto free_cpufreq;
         }
@@ -978,9 +984,9 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
         /* create device files */
         ret = device_create_file(&pdev->dev, &dev_attr_debug);
         if (ret)
-                printk(KERN_ERR "failed to add debug attribute\n");
+                dev_err(&pdev->dev, "failed to add debug attribute\n");
 
-        printk(KERN_INFO "fb%d: %s frame buffer device\n",
+        dev_info(&pdev->dev, "fb%d: %s frame buffer device\n",
                 fbinfo->node, fbinfo->fix.id);
 
         return 0;
@@ -1028,7 +1034,7 @@ static int __devexit s3c2410fb_remove(struct platform_device *pdev)
         s3c2410fb_cpufreq_deregister(info);
 
         s3c2410fb_lcd_enable(info, 0);
-        usleep_range(1000, 1000);
+        usleep_range(1000, 1100);
 
         s3c2410fb_unmap_video_memory(fbinfo);
 
@@ -1065,7 +1071,7 @@ static int s3c2410fb_suspend(struct platform_device *dev, pm_message_t state)
          * the LCD DMA engine is not going to get back on the bus
          * before the clock goes off again (bjd) */
 
-        usleep_range(1000, 1000);
+        usleep_range(1000, 1100);
         clk_disable(info->clk);
 
         return 0;
@@ -1077,7 +1083,7 @@ static int s3c2410fb_resume(struct platform_device *dev)
         struct s3c2410fb_info *info = fbinfo->par;
 
         clk_enable(info->clk);
-        usleep_range(1000, 1000);
+        usleep_range(1000, 1100);
 
         s3c2410fb_init_registers(fbinfo);
 
@@ -1134,8 +1140,8 @@ static void __exit s3c2410fb_cleanup(void)
 module_init(s3c2410fb_init);
 module_exit(s3c2410fb_cleanup);
 
-MODULE_AUTHOR("Arnaud Patard <arnaud.patard@rtp-net.org>, "
-              "Ben Dooks <ben-linux@fluff.org>");
+MODULE_AUTHOR("Arnaud Patard <arnaud.patard@rtp-net.org>");
+MODULE_AUTHOR("Ben Dooks <ben-linux@fluff.org>");
 MODULE_DESCRIPTION("Framebuffer driver for the s3c2410");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:s3c2410-lcd");
diff --git a/drivers/video/savage/savagefb_driver.c b/drivers/video/savage/savagefb_driver.c
index 0d0f52c18fd8..f4f53b082d05 100644
--- a/drivers/video/savage/savagefb_driver.c
+++ b/drivers/video/savage/savagefb_driver.c
@@ -2266,8 +2266,10 @@ static int __devinit savagefb_probe(struct pci_dev* dev,
         lpitch = info->var.xres_virtual*((info->var.bits_per_pixel + 7) >> 3);
         info->var.yres_virtual = info->fix.smem_len/lpitch;
 
-        if (info->var.yres_virtual < info->var.yres)
+        if (info->var.yres_virtual < info->var.yres) {
+                err = -ENOMEM;
                 goto failed;
+        }
 
 #if defined(CONFIG_FB_SAVAGE_ACCEL)
         /*
diff --git a/drivers/video/sis/initextlfb.c b/drivers/video/sis/initextlfb.c
index 9dec64da4015..3ab18f5a3759 100644
--- a/drivers/video/sis/initextlfb.c
+++ b/drivers/video/sis/initextlfb.c
@@ -65,7 +65,7 @@ sisfb_mode_rate_to_dclock(struct SiS_Private *SiS_Pr, unsigned char modeno,
     }
 #endif
 
-    if(!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex))) {;
+    if(!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex))) {
        printk(KERN_ERR "Could not find mode %x\n", ModeNo);
        return 65000;
     }
diff --git a/drivers/video/sunxvr1000.c b/drivers/video/sunxvr1000.c
index b7f27acaf817..729a50722bdf 100644
--- a/drivers/video/sunxvr1000.c
+++ b/drivers/video/sunxvr1000.c
@@ -141,8 +141,10 @@ static int __devinit gfb_probe(struct platform_device *op)
 
         gp->fb_base = of_ioremap(&op->resource[6], 0,
                                  gp->fb_size, "gfb fb");
-        if (!gp->fb_base)
+        if (!gp->fb_base) {
+                err = -ENOMEM;
                 goto err_release_fb;
+        }
 
         err = gfb_set_fbinfo(gp);
         if (err)
diff --git a/drivers/video/sunxvr2500.c b/drivers/video/sunxvr2500.c
index 5848436c19da..7fbcba86d1a2 100644
--- a/drivers/video/sunxvr2500.c
+++ b/drivers/video/sunxvr2500.c
@@ -181,8 +181,10 @@ static int __devinit s3d_pci_register(struct pci_dev *pdev,
         sp->fb_size = info->fix.line_length * sp->height;
 
         sp->fb_base = ioremap(sp->fb_base_phys, sp->fb_size);
-        if (!sp->fb_base)
+        if (!sp->fb_base) {
+                err = -ENOMEM;
                 goto err_release_pci;
+        }
 
         err = s3d_set_fbinfo(sp);
         if (err)
diff --git a/drivers/video/sunxvr500.c b/drivers/video/sunxvr500.c
index eb931b8626fa..6c71b1b44477 100644
--- a/drivers/video/sunxvr500.c
+++ b/drivers/video/sunxvr500.c
@@ -298,8 +298,10 @@ static int __devinit e3d_pci_register(struct pci_dev *pdev,
                 goto err_release_fb;
         }
         ep->ramdac = ioremap(ep->regs_base_phys + 0x8000, 0x1000);
-        if (!ep->ramdac)
+        if (!ep->ramdac) {
+                err = -ENOMEM;
                 goto err_release_pci1;
+        }
 
         ep->fb8_0_off = readl(ep->ramdac + RAMDAC_VID_8FB_0);
         ep->fb8_0_off -= ep->fb_base_reg;
@@ -343,8 +345,10 @@ static int __devinit e3d_pci_register(struct pci_dev *pdev,
         ep->fb_size = info->fix.line_length * ep->height;
 
         ep->fb_base = ioremap(ep->fb_base_phys, ep->fb_size);
-        if (!ep->fb_base)
+        if (!ep->fb_base) {
+                err = -ENOMEM;
                 goto err_release_pci0;
+        }
 
         err = e3d_set_fbinfo(ep);
         if (err)
diff --git a/drivers/video/udlfb.c b/drivers/video/udlfb.c
index f45eba3d6150..86d449ea3169 100644
--- a/drivers/video/udlfb.c
+++ b/drivers/video/udlfb.c
@@ -646,7 +646,7 @@ static ssize_t dlfb_ops_write(struct fb_info *info, const char __user *buf,
         result = fb_sys_write(info, buf, count, ppos);
 
         if (result > 0) {
-                int start = max((int)(offset / info->fix.line_length) - 1, 0);
+                int start = max((int)(offset / info->fix.line_length), 0);
                 int lines = min((u32)((result / info->fix.line_length) + 1),
                                 (u32)info->var.yres);
 
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index b0e2a4261afe..2f8f82d874a1 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -659,6 +659,8 @@ static int __devinit uvesafb_vbe_getedid(struct uvesafb_ktask *task,
         task->t.flags = TF_BUF_RET | TF_BUF_ESDI;
         task->t.buf_len = EDID_LENGTH;
         task->buf = kzalloc(EDID_LENGTH, GFP_KERNEL);
+        if (!task->buf)
+                return -ENOMEM;
 
         err = uvesafb_exec(task);
 
diff --git a/drivers/video/vermilion/vermilion.c b/drivers/video/vermilion/vermilion.c
index 89aef343e295..4709edc3cb7f 100644
--- a/drivers/video/vermilion/vermilion.c
+++ b/drivers/video/vermilion/vermilion.c
@@ -1167,8 +1167,7 @@ void vmlfb_unregister_subsys(struct vml_sys *sys)
         list_for_each_entry_safe(entry, next, &global_has_mode, head) {
                 printk(KERN_DEBUG MODULE_NAME ": subsys disable pipe\n");
                 vmlfb_disable_pipe(entry);
-                list_del(&entry->head);
-                list_add_tail(&entry->head, &global_no_mode);
+                list_move_tail(&entry->head, &global_no_mode);
         }
         mutex_unlock(&vml_mutex);
 }
diff --git a/drivers/video/via/via_clock.c b/drivers/video/via/via_clock.c
index af8f26b643c1..db1e39277e32 100644
--- a/drivers/video/via/via_clock.c
+++ b/drivers/video/via/via_clock.c
@@ -25,6 +25,7 @@
 
 #include <linux/kernel.h>
 #include <linux/via-core.h>
+#include <asm/olpc.h>
 #include "via_clock.h"
 #include "global.h"
 #include "debug.h"
@@ -289,6 +290,10 @@ static void dummy_set_pll(struct via_pll_config config)
         printk(KERN_INFO "Using undocumented set PLL.\n%s", via_slap);
 }
 
+static void noop_set_clock_state(u8 state)
+{
+}
+
 void via_clock_init(struct via_clock *clock, int gfx_chip)
 {
         switch (gfx_chip) {
@@ -346,4 +351,18 @@ void via_clock_init(struct via_clock *clock, int gfx_chip)
                 break;
 
         }
+
+        if (machine_is_olpc()) {
+                /* The OLPC XO-1.5 cannot suspend/resume reliably if the
+                 * IGA1/IGA2 clocks are set as on or off (memory rot
+                 * occasionally happens during suspend under such
+                 * configurations).
+                 *
+                 * The only known stable scenario is to leave this bits as-is,
+                 * which in their default states are documented to enable the
+                 * clock only when it is needed.
+                 */
+                clock->set_primary_clock_state = noop_set_clock_state;
+                clock->set_secondary_clock_state = noop_set_clock_state;
+        }
 }
diff --git a/drivers/video/xen-fbfront.c b/drivers/video/xen-fbfront.c
index b7f5173ff9e9..917bb5681684 100644
--- a/drivers/video/xen-fbfront.c
+++ b/drivers/video/xen-fbfront.c
@@ -641,7 +641,6 @@ static void xenfb_backend_changed(struct xenbus_device *dev,
         case XenbusStateReconfiguring:
         case XenbusStateReconfigured:
         case XenbusStateUnknown:
-        case XenbusStateClosed:
                 break;
 
         case XenbusStateInitWait:
@@ -670,6 +669,10 @@ InitWait:
                 info->feature_resize = val;
                 break;
 
+        case XenbusStateClosed:
+                if (dev->state == XenbusStateClosed)
+                        break;
+                /* Missed the backend's CLOSING state -- fallthrough */
         case XenbusStateClosing:
                 xenbus_frontend_closed(dev);
                 break;
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index d4dffcd52873..126d8ce591ce 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -3,6 +3,7 @@ menu "Xen driver support"
 
 config XEN_BALLOON
         bool "Xen memory balloon driver"
+        depends on !ARM
         default y
         help
           The balloon driver allows the Xen domain to request more memory from
@@ -145,6 +146,7 @@ config SWIOTLB_XEN
 
 config XEN_TMEM
         bool
+        depends on !ARM
         default y if (CLEANCACHE || FRONTSWAP)
         help
           Shim to interface in-kernel Transcendent Memory hooks
diff --git a/drivers/xen/balloon.c b/drivers/xen/balloon.c
index 31ab82fda38a..d6886d90ccfd 100644
--- a/drivers/xen/balloon.c
+++ b/drivers/xen/balloon.c
@@ -55,7 +55,6 @@
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
 #include <asm/tlb.h>
-#include <asm/e820.h>
 
 #include <asm/xen/hypervisor.h>
 #include <asm/xen/hypercall.h>
@@ -88,7 +87,7 @@ struct balloon_stats balloon_stats;
 EXPORT_SYMBOL_GPL(balloon_stats);
 
 /* We increase/decrease in batches which fit in a page */
-static unsigned long frame_list[PAGE_SIZE / sizeof(unsigned long)];
+static xen_pfn_t frame_list[PAGE_SIZE / sizeof(unsigned long)];
 
 #ifdef CONFIG_HIGHMEM
 #define inc_totalhigh_pages() (totalhigh_pages++)
diff --git a/drivers/xen/dbgp.c b/drivers/xen/dbgp.c
index 42569c77ccc8..f3ccc80a455f 100644
--- a/drivers/xen/dbgp.c
+++ b/drivers/xen/dbgp.c
@@ -8,7 +8,9 @@
 
 static int xen_dbgp_op(struct usb_hcd *hcd, int op)
 {
+#ifdef CONFIG_PCI
         const struct device *ctrlr = hcd_to_bus(hcd)->controller;
+#endif
         struct physdev_dbgp_op dbgp;
 
         if (!xen_initial_domain())
diff --git a/drivers/xen/events.c b/drivers/xen/events.c
index 59e10a1286d5..912ac81b6dbf 100644
--- a/drivers/xen/events.c
+++ b/drivers/xen/events.c
@@ -115,7 +115,9 @@ struct irq_info {
 #define PIRQ_SHAREABLE  (1 << 1)
 
 static int *evtchn_to_irq;
+#ifdef CONFIG_X86
 static unsigned long *pirq_eoi_map;
+#endif
 static bool (*pirq_needs_eoi)(unsigned irq);
 
 static DEFINE_PER_CPU(unsigned long [NR_EVENT_CHANNELS/BITS_PER_LONG],
@@ -277,10 +279,12 @@ static unsigned int cpu_from_evtchn(unsigned int evtchn)
         return ret;
 }
 
+#ifdef CONFIG_X86
 static bool pirq_check_eoi_map(unsigned irq)
 {
         return test_bit(pirq_from_irq(irq), pirq_eoi_map);
 }
+#endif
 
 static bool pirq_needs_eoi_flag(unsigned irq)
 {
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index 610bfc6be177..2e22df2f7a3f 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -105,6 +105,21 @@ static void gntdev_print_maps(struct gntdev_priv *priv,
 #endif
 }
 
+static void gntdev_free_map(struct grant_map *map)
+{
+        if (map == NULL)
+                return;
+
+        if (map->pages)
+                free_xenballooned_pages(map->count, map->pages);
+        kfree(map->pages);
+        kfree(map->grants);
+        kfree(map->map_ops);
+        kfree(map->unmap_ops);
+        kfree(map->kmap_ops);
+        kfree(map);
+}
+
 static struct grant_map *gntdev_alloc_map(struct gntdev_priv *priv, int count)
 {
         struct grant_map *add;
@@ -142,12 +157,7 @@ static struct grant_map *gntdev_alloc_map(struct gntdev_priv *priv, int count)
         return add;
 
 err:
-        kfree(add->pages);
-        kfree(add->grants);
-        kfree(add->map_ops);
-        kfree(add->unmap_ops);
-        kfree(add->kmap_ops);
-        kfree(add);
+        gntdev_free_map(add);
         return NULL;
 }
 
@@ -198,17 +208,9 @@ static void gntdev_put_map(struct grant_map *map)
                 evtchn_put(map->notify.event);
         }
 
-        if (map->pages) {
-                if (!use_ptemod)
-                        unmap_grant_pages(map, 0, map->count);
-
-                free_xenballooned_pages(map->count, map->pages);
-        }
-        kfree(map->pages);
-        kfree(map->grants);
-        kfree(map->map_ops);
-        kfree(map->unmap_ops);
-        kfree(map);
+        if (map->pages && !use_ptemod)
+                unmap_grant_pages(map, 0, map->count);
+        gntdev_free_map(map);
 }
 
 /* ------------------------------------------------------------------ */
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index b2b0a375b348..b91f14e83164 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -84,7 +84,7 @@ struct gnttab_ops {
          * nr_gframes is the number of frames to map grant table. Returning
          * GNTST_okay means success and negative value means failure.
          */
-        int (*map_frames)(unsigned long *frames, unsigned int nr_gframes);
+        int (*map_frames)(xen_pfn_t *frames, unsigned int nr_gframes);
         /*
          * Release a list of frames which are mapped in map_frames for grant
          * entry status.
@@ -960,7 +960,7 @@ static unsigned nr_status_frames(unsigned nr_grant_frames)
         return (nr_grant_frames * GREFS_PER_GRANT_FRAME + SPP - 1) / SPP;
 }
 
-static int gnttab_map_frames_v1(unsigned long *frames, unsigned int nr_gframes)
+static int gnttab_map_frames_v1(xen_pfn_t *frames, unsigned int nr_gframes)
 {
         int rc;
 
@@ -977,7 +977,7 @@ static void gnttab_unmap_frames_v1(void)
         arch_gnttab_unmap(gnttab_shared.addr, nr_grant_frames);
 }
 
-static int gnttab_map_frames_v2(unsigned long *frames, unsigned int nr_gframes)
+static int gnttab_map_frames_v2(xen_pfn_t *frames, unsigned int nr_gframes)
 {
         uint64_t *sframes;
         unsigned int nr_sframes;
@@ -1029,7 +1029,7 @@ static void gnttab_unmap_frames_v2(void)
 static int gnttab_map(unsigned int start_idx, unsigned int end_idx)
 {
         struct gnttab_setup_table setup;
-        unsigned long *frames;
+        xen_pfn_t *frames;
         unsigned int nr_gframes = end_idx + 1;
         int rc;
 
diff --git a/drivers/xen/sys-hypervisor.c b/drivers/xen/sys-hypervisor.c
index 5e5ad7e28858..96453f8a85c5 100644
--- a/drivers/xen/sys-hypervisor.c
+++ b/drivers/xen/sys-hypervisor.c
@@ -11,6 +11,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/kobject.h>
+#include <linux/err.h>
 
 #include <asm/xen/hypervisor.h>
 #include <asm/xen/hypercall.h>
@@ -284,7 +285,8 @@ static ssize_t virtual_start_show(struct hyp_sysfs_attr *attr, char *buffer)
                 ret = HYPERVISOR_xen_version(XENVER_platform_parameters,
                                              parms);
                 if (!ret)
-                        ret = sprintf(buffer, "%lx\n", parms->virt_start);
+                        ret = sprintf(buffer, "%"PRI_xen_ulong"\n",
+                                      parms->virt_start);
                 kfree(parms);
         }
 
diff --git a/drivers/xen/xen-pciback/vpci.c b/drivers/xen/xen-pciback/vpci.c
index 46d140baebd8..0f478ac483cd 100644
--- a/drivers/xen/xen-pciback/vpci.c
+++ b/drivers/xen/xen-pciback/vpci.c
@@ -89,9 +89,15 @@ static int __xen_pcibk_add_pci_dev(struct xen_pcibk_device *pdev,
 
         mutex_lock(&vpci_dev->lock);
 
-        /* Keep multi-function devices together on the virtual PCI bus */
-        for (slot = 0; slot < PCI_SLOT_MAX; slot++) {
-                if (!list_empty(&vpci_dev->dev_list[slot])) {
+        /*
+         * Keep multi-function devices together on the virtual PCI bus, except
+         * virtual functions.
+         */
+        if (!dev->is_virtfn) {
+                for (slot = 0; slot < PCI_SLOT_MAX; slot++) {
+                        if (list_empty(&vpci_dev->dev_list[slot]))
+                                continue;
+
                         t = list_entry(list_first(&vpci_dev->dev_list[slot]),
                                        struct pci_dev_entry, list);
 
@@ -116,7 +122,7 @@ static int __xen_pcibk_add_pci_dev(struct xen_pcibk_device *pdev,
                                pci_name(dev), slot);
                         list_add_tail(&dev_entry->list,
                                       &vpci_dev->dev_list[slot]);
-                        func = PCI_FUNC(dev->devfn);
+                        func = dev->is_virtfn ? 0 : PCI_FUNC(dev->devfn);
                         goto unlock;
                 }
         }
diff --git a/drivers/xen/xenbus/xenbus_dev_frontend.c b/drivers/xen/xenbus/xenbus_dev_frontend.c
index 89f76252a16f..ac727028e658 100644
--- a/drivers/xen/xenbus/xenbus_dev_frontend.c
+++ b/drivers/xen/xenbus/xenbus_dev_frontend.c
@@ -458,7 +458,7 @@ static ssize_t xenbus_file_write(struct file *filp,
                 goto out;
 
         /* Can't write a xenbus message larger we can buffer */
-        if ((len + u->len) > sizeof(u->u.buffer)) {
+        if (len > sizeof(u->u.buffer) - u->len) {
                 /* On error, dump existing buffer */
                 u->len = 0;
                 rc = -EINVAL;
diff --git a/drivers/xen/xenbus/xenbus_xs.c b/drivers/xen/xenbus/xenbus_xs.c
index 131dec04794e..acedeabe589c 100644
--- a/drivers/xen/xenbus/xenbus_xs.c
+++ b/drivers/xen/xenbus/xenbus_xs.c
@@ -48,6 +48,7 @@
 #include <xen/xenbus.h>
 #include <xen/xen.h>
 #include "xenbus_comms.h"
+#include <asm/xen/hypervisor.h>
 
 struct xs_stored_msg {
         struct list_head list;
@@ -618,7 +619,26 @@ static struct xenbus_watch *find_watch(const char *token)
 
         return NULL;
 }
+/*
+ * Certain older XenBus toolstack cannot handle reading values that are
+ * not populated. Some Xen 3.4 installation are incapable of doing this
+ * so if we are running on anything older than 4 do not attempt to read
+ * control/platform-feature-xs_reset_watches.
+ */
+static bool xen_strict_xenbus_quirk(void)
+{
+#ifdef CONFIG_X86
+        uint32_t eax, ebx, ecx, edx, base;
+
+        base = xen_cpuid_base();
+        cpuid(base + 1, &eax, &ebx, &ecx, &edx);
 
+        if ((eax >> 16) < 4)
+                return true;
+#endif
+        return false;
+
+}
 static void xs_reset_watches(void)
 {
         int err, supported = 0;
@@ -626,6 +646,9 @@ static void xs_reset_watches(void)
         if (!xen_hvm_domain() || xen_initial_domain())
                 return;
 
+        if (xen_strict_xenbus_quirk())
+                return;
+
         err = xenbus_scanf(XBT_NIL, "control",
                         "platform-feature-xs_reset_watches", "%d", &supported);
         if (err != 1 || !supported)